#!/usr/bin/env python
# -*- coding: UTF-8 -*-
import sys
import numpy as np
import cv2
import rospy
from std_msgs.msg import Int32
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from ackermann_msgs.msg import AckermannDriveStamped
from geometry_msgs.msg import Twist
from enum import Enum
import math

# 使用时间模块
import time

# 引入元素识别模块
import The_first_schemes_for_sign_detect as first_detect

# 引入红绿灯识别模块
import Red_Green_detect as RGD

# 引入PID控制模块
import my_PID as pid

# 引入巡线图像处理模块(灰度/HSV)
# 巡线图像查看模块1——灰度
import Capture_lane_image

# 巡线图像查看模块2——HSV
import HSV_Capture_lane_image

# 引入帅男巡线模块+蓝线校准模块
import xunxian_final as xunxian

# 引入歪果巡线模块
import wig_lane_dec

# 引入歪果蓝线模块检测停止指令
import get_blue_line as blue_line
import stop_by_blue_lind as stop_blue

# 帅男的蓝线检测出现问题
# 现在尝试更换摄像头位置之后的情况
# 引入蓝线检测积分模式


# 引入侧方视觉辅助矫正模块
import cefang_line_detect as cefang

# 10.19,将电机的符号位加上了，而且去掉了大部分的sleep
# ---------------------------------------------------------
###小车状态全局参数定义，帅男代码风格
'''电机极性参数start'''
dianji_fuhao = -1  # 电机正负号，1表示正，-1表示负
'''电机极性参数end'''

''' 与避障有关参数start'''
data_cmd = Int32()  # 激光雷达控制代码,主节点告诉雷达节点应该执行避障还是入库
bizhang_callback_data = 0  # 雷达节点发过来，为1表示请求避障
''' 与避障有关参数end'''

''' 与红绿灯有关参数start'''
honglvdeng_shibie_score = 0
start_flag = 1  # 红绿灯发车标志位，为1表示发车，为0表示不发车
''' 与红绿灯有关参数end'''

''' 与左转有关参数start'''
left_big_flag = 1  # 小车左转标志位，为1表示第一阶段，为2表示第二阶段，为3表示第三阶段
left_big_flag_fast = 1  # 小车左大转的快速标志位，使用这个元素，可以在结束左转之后有很快的速度
left_big_flag_border = 1  # 边界左大的标志位
left_small_flag = 1  # 小车左转标志位，为1表示第一阶段，为2表示第二阶段，为3表示第三阶段
left_small_flag_fast = 1  # # 小车左小转快速标志位，使用这个元素，可以在结束左转之后有很快的速度
left_jiaodu_now = 0  # 小车左转车头当前角度
left_jiaodu_last = 0  # 小车左转车头开始时刻角度
''' 与左转有关参数end'''

''' 与右转有关参数start'''
right_big_flag = 1  # 小车右转标志位，为1表示第一阶段，为2表示第二阶段，为3表示第三阶段
right_big_flag_fast = 1  # 小车左大转的快速标志位，使用这个元素，可以在结束左转之后有很快的速度
right_big_flag_border = 1  # 边界右大的标志位
right_small_flag = 1  # 小车右转标志位，为1表示第一阶段，为2表示第二阶段，为3表示第三阶段
right_small_flag_fast = 1  # # 小车左小转快速标志位，使用这个元素，可以在结束左转之后有很快的速度
right_jiaodu_now = 0  # 小车右转车头当前角度
right_jiaodu_last = 0  # 小车右转车头开始时刻角度
''' 与右转有关参数end'''

''' 与直行有关参数start'''
straight_flag = 1  # 小车直行标志位，为1表示第一阶段，为2表示第二阶段，为3表示第三阶段
straight_flag_extra = 1  # 额外直行的标志位参数
''' 与直行有关参数end'''

''' 与调头有关参数start'''
diaotou_flag = 1  # 小车调头标志位，为1表示第一阶段，为2表示第二阶段，为3表示第三阶段
diaotou_jiaodu_now = 0  # 小车调头车头当前角度
diaotou_jiaodu_last = 0  # 小车调头车头开始时刻角度
tiaozheng_cishu = 7  # 小车调头矫正次数

diaotou_boundary_flag = 1  # 边界掉头标志位

''' 与调头有关参数end'''

''' 与下位机陀螺仪有关参数start'''
jiaodu_di = 0  # 小车当前角度低位
jiaodu_gao = 0  # 小车角度高位
jiaodu_now = 0  # 小车当前角度
speed_now = 0  # 小车当前速度
''' 与下位机陀螺仪有关参数end'''

''' 与倒车入库有关的参数start'''
daoche_ruku_shibie_score = 0
daocheruku_flag = 1  # 小车倒车入库标志位，为1表示第一阶段，为2表示第二阶段，为3表示第三阶段
'''与倒车入库有关的参数end'''

''' 与正车入库有关的参数start'''
zhengcheruku_flag = 1  # 小车正车入库标志位，为1表示第一阶段，为2表示第二阶段，为3表示第三阶段
'''与正车入库有关的参数end'''

''' 与侧方停车有关的参数start'''
cefangtingche_flag = 1  # 小车侧方停车步骤标志位
cefang_start_time = 0  # 侧方停车的起始时间
cefang_judge_flag = 1  # 侧方停车判断标志位
'''与侧方停车有关的参数end'''


# ---------------------------------------------------------
# 一些枚举数据类型的定义


# 道路类型定义
# 这里的数字没有意义
class Road_Type(Enum):
    XUNXIAN = 0  # 巡线
    BIZHANG = 1  # 避障（内）
    BIZHANG_WAI = 110  # 避障（外）
    HONGLVDENG = 4  # 红绿灯
    PAUSE = 8  # 停止
    DIAOTOU = 9  # 掉头
    DIAOTOU_BOUNDARY = 99  # 边界参数掉头

    LEFT_BIG = 5  # 左大转
    LEFT_BIG_FAST = 55  # 快速左大转
    LEFT_BIG_BORDER = 555  # 边界参数左大转（快速）
    LEFT_SMALL = 10  # 左小转
    LEFT_SMALL_FAST = 1010  # 快速左小转

    RIGHT_BIG = 6  # 右大转
    RIGHT_BIG_FAST = 66  # 快速右大转
    RIGHT_BIG_BORDER = 666  # 边界参数右大转（快速）
    RIGHT_SMALL = 11  # 右小转
    RIGHT_SMALL_FAST = 1111  # 快速右小转

    STRAIGHT = 7  # 直行
    # 这里加上了额外的直走道路模式
    # 9.8
    STRAIGHT_EXTRA = 77

    CEFANGTINGCHE = 13  # 侧方停车

    # 10.16,增加了交叉入库的道路模式

    # 正车--从左往右扫牌子
    ZHENGCHERUKU_l2r = 12
    # 正车（右——左）
    JC_RUKU_RTL_FRONT = 16
    # 正车（左——右）
    JC_RUKU_LTR_FRONT = 17
    # 正车--从右往左扫牌子
    ZHENGCHERUKU_r2l = 19

    # 倒车（右——左）
    JC_RUKU_RTL_BACK = 14
    # 倒车（左——右）
    JC_RUKU_LTR_BACK = 15
    # 倒车--从右往左扫牌子
    DAOCHERUKU_r2l = 18
    # 倒车从左往右扫牌子
    DAOCHERUKU_l2r = 2


# 小车模式定义


class Car_Mode(Enum):
    Normal = 1
    Test = 2
    Stop = 3


###自适应增量式PID，用于上位机速度闭环
class IncrementalPIDController:
    def __init__(self, kp_base, kd_base, ki_base, setpoint, max_output=None, max_integral=None):
        self.kp_base = kp_base  # 基础比例系数
        self.kd_base = kd_base  # 基础微分系数
        self.ki_base = ki_base  # 基础积分系数
        self.setpoint = setpoint  # 目标设定值
        self.prev_error = 0  # 上一次的误差
        self.integral = 0  # 积分项
        self.max_output = max_output  # 控制信号最大值
        self.max_integral = max_integral  # 积分项最大值

    def compute(self, current_speed):
        error = self.setpoint - current_speed
        delta_error = error - self.prev_error

        # 动态调整 kp 和 kd 和 ki
        kp = self.dynamic_kp(error)
        kd = self.dynamic_kd(delta_error)
        ki = self.dynamic_ki(error)

        # 积分项累积并限幅
        self.integral += error
        if self.max_integral is not None:
            self.integral = max(min(self.integral, self.max_integral), -self.max_integral)

        # PID控制器的增量计算
        delta_output = kp * error + kd * delta_error + ki * self.integral
        self.prev_error = error

        # 限制控制信号的输出
        if self.max_output is not None:
            delta_output = max(min(delta_output, self.max_output), -self.max_output)

        return delta_output

    def dynamic_kp(self, error):
        # 使用非线性函数调整 kp，使其对误差变化更加平滑
        return self.kp_base * (1 - np.exp(-0.1 * abs(error)))

    def dynamic_kd(self, delta_error):
        # 使用非线性函数调整 kd，使其对误差变化更加平滑
        return self.kd_base * (1 - np.exp(-0.1 * abs(delta_error)))

    def dynamic_ki(self, error):
        # 使用非线性函数调整 ki，使其对误差变化更加平滑
        return self.ki_base * (1 - np.exp(-0.1 * abs(error)))


# 常用全局参数，下面是wiggle声明全局变量的风格，注意区分
# 创建发布对象
global pub, cmd_pub
# 其中msg是小车巡线控制代码，data是发向激光雷达的控制代码
global msg, data
# 后退标志
global back
# 小车默认的道路模式
global road_type
# 小车的工作模式
global car_mode
# 小车元素表
global Element_Table
# 状态表默认索引
global Element_Table_Index
global Element_Table_last_Index  # (过去)

# 执行蓝线修正的时间
global fix_time
# 小车的校0角度
global angle_zero

# 蓝线检测积分
global blue_line_score

# 侧方竖直直线矫正的步骤符号---------------------------------
global cefang_Longitudinal_flag
# 引入侧方直线检测的一些参数
global points_1, points_2, H_cefang_line, H_F_slope
# 侧方执行步骤的标志
global cefang_execute_flag
# 侧方局部动作执行的一些标志
global long_state_flag
# 侧方横向修正动作标志
global cefang_hengxiang_flag
# x方向上的误差参数
global x_error
# 图片序列等待长度
global picture_count
# 侧方白线的条数
global white_lines_num
# 侧方白线的位置记录参数
global white_lines_num_record
# 侧方选择位置的标志参数
global set_location_flag
# 侧方停车的开始计数器
global cefang_start_count

# 交叉入库的相关标志变量
# 用于控制扫描竖线的标志位
global jiaocha_ruku_H_line_detect_flag
# 竖线检测出来的数据参数
global jiaocha_ruku_H_line
# 竖线检测时候的卷积次数全局设定
global jiaocha_ruku_H_line_iterations
# 交叉入库卷积核的设定
global kernel_set_jiaocha_ruku_line
# 交叉入库斜率巡线的相关参数
global jiaocha_ruku_line, jiaocha_ruku_H_slope
# 交叉入库的执行标志位
global JC_daoche_ruku_Execute_flag
# 动作组执行标志位1
global action_l_t_r
# 动作组直行标志位2
global action_r_t_l
# 动作组直行标志位3
global action_l_t_r_front
# 动作组直行标志位4
global action_r_t_l_front

# 交叉入库用到的控制阶段标志位
# 倒车（右——左）
global jc_ruku_rtl_back_flag
# 倒车（左——右）
global jc_ruku_ltr_back_flag
# 正车（右——左）
global jc_ruku_rtl_front_flag
# 正车（左——右）
global jc_ruku_ltr_front_flag

# 内道避障的执行标志
global bizhang_nei_exe_flag

# 内道避障所使用的全局时间
global nei_start_time


# 小车参数初始化函数
def Car_paragram_init():
    # 创建发布对象
    global pub, cmd_pub
    # 其中msg是小车巡线控制代码，data是发向激光雷达的控制代码
    global msg, data
    # 后退标志
    global back
    # 小车默认的道路模式
    global road_type
    # 小车的工作模式
    global car_mode
    # 小车元素表
    global Element_Table
    # 状态表默认索引
    global Element_Table_Index
    global Element_Table_last_Index  # (过去)
    # 执行蓝线修正的时间
    global fix_time
    # 小车的校0角度
    global angle_zero
    # 蓝线检测积分
    global blue_line_score
    # --------------------------------------------------
    # 侧方竖直直线矫正的步骤符号
    global cefang_Longitudinal_flag
    # 引入直线检测的一些参数
    global points_1, points_2, H_cefang_line, H_F_slope
    # 侧方执行步骤的标志
    global cefang_execute_flag
    # 侧方局部动作执行的一些标志
    global long_state_flag
    # 侧方横向修正动作标志
    global cefang_hengxiang_flag
    # x方向上的误差参数
    global x_error
    # 图片序列等待长度
    global picture_count

    # 侧方视觉选择位置的方案使用的参数，可能采用帅男的雷达方案，此视觉方案备用
    global white_lines_num  # 侧方白线的条数，选择侧方停车的格子，因此初始化参数很重要
    # 侧方白线的位置记录参数
    global white_lines_num_record

    global set_location_flag  # 侧方选择位置的标志参数

    # 侧方停车的开始计数器
    global cefang_start_count

    # 交叉入库的相关标志变量
    # 用于控制扫描竖线的标志位
    global jiaocha_ruku_H_line_detect_flag
    # 竖线检测出来的数据参数
    global jiaocha_ruku_H_line
    # 竖线检测时候的卷积次数全局设定
    global jiaocha_ruku_H_line_iterations
    # 交叉入库卷积核的设定
    global kernel_set_jiaocha_ruku_line
    # 交叉入库斜率巡线的相关参数
    global jiaocha_ruku_line, jiaocha_ruku_H_slope
    # 交叉入库的执行标志位
    global JC_daoche_ruku_Execute_flag
    # 动作组执行标志位1
    global action_l_t_r
    # 动作组执行标志位2
    global action_r_t_l
    # 动作组直行标志位3
    global action_l_t_r_front
    # 动作组直行标志位4
    global action_r_t_l_front

    # 交叉入库用到的控制阶段标志位
    # 倒车（右——左）
    global jc_ruku_rtl_back_flag
    # 倒车（左——右）
    global jc_ruku_ltr_back_flag
    # 正车（右——左）
    global jc_ruku_rtl_front_flag
    # 正车（左——右）
    global jc_ruku_ltr_front_flag

    # 内道避障的执行标志
    global bizhang_nei_exe_flag

    # 内道避障所使用的全局时间
    global nei_start_time

    # 倒车标志初始化
    back = 0

    # 小车传向下位机的数据
    msg = AckermannDriveStamped()

    # 设置小车当前的工作模式
    car_mode = Car_Mode.Normal

    # 小车的默认的道路模式
    road_type = Road_Type.XUNXIAN

    # 元素表初始化设置
    Element_Table = [0 for i in range(20)]  # 元素表初始化设置,给了20个0

    # 元素表说明，按照功能，并不是按照数字顺序

    # 0.代表不使用元素表，进入基础巡线控制

    # --------特殊的功能------
    # 3.代表红绿灯识别，
    # 1.代表避障(内道)
    # 110.避障（外道）
    # 7.调头识别，
    # 77.边界掉头      # 一般就使用边界掉头就可以了

    # --------左转------------
    # 4.左转大圈，--- 44.快速左大（无特殊巡线） ---444.边界左大（在左大经过边界的时候，可以采用这一套参数，避免压边）
    # 8.左转小圈，--- 88.快速左小（无特殊巡线）

    # --------右转------------
    # 5.右转大圈，--- 55.快速右大（无特殊巡线） ---555.边界右大（在右大经过边界的时候，可以采用这一套参数，避免压边）
    # 9.右转小圈. --- 99.快速右小（无特殊巡线）

    # --------直行------------
    # 6.代表直行识别1，
    # 66.代表直行识别2.，这两个需要一起使用

    # --------倒车入库--------
    # 2.倒车入库--从左往右扫描牌子，这个是看牌子的方向
    # 16.倒车入库--从右往左扫描牌子
    # 12.交叉倒车（右——左），这个带括号的方向，是指车子面对的方向
    # 13.交叉倒车（左——右）

    # --------正车入库--------
    # 10.正车入库--从左往右扫描牌子
    # 17.正车入库--从右往左扫描牌子
    # 14.交叉正车（右——左）
    # 15.交叉正车（左——右）

    # --------特殊的停车--------
    # 11.侧方停车

    # 手动传参（无法更改代码的时候使用）
    # 这里就是主类构造函数的传参，如果传参了，就使用传参的参数，否则使用默认的参数
    if len(sys.argv) > 1:
        for i in range(len(sys.argv) - 1):
            Element_Table[i] = int(sys.argv[i + 1])
    else:
        # 这里就是没有使用外部传参命令的时候，这里就使用提前设置好的元素表

        ###############
        # 听着，如果侧方的牌子，跟你预想的位置不一样，那么你在侧方judge当中的积分数值也需要进行更改，这点千万不能忘记

        # 一些常规的动作测试
        # Element_Table = [1, 0]    # 避障(内)
        # Element_Table = [11, 0]   # 避障(外)
        # Element_Table = [99, 11, 0]   # 右小 + 侧方停车
        Element_Table = [6, 66, 11, 0]  # 直走 + 侧方停车
        # Element_Table = [9, 10, 0]     # 正车入库测试
        # Element_Table = [9, 2, 0]      # 右小 + 倒车
        # Element_Table = [8, 5, 0]       # 左小 + 右大
        # Element_Table = [4, 9, 11, 0]       # 左大 + 右小 + 侧方
        # Element_Table = [8, 2, 0]       # 左小+倒车入库
        # Element_Table = [4, 10, 0]       # 左小+倒车入库
        # Element_Table = [7, 0]            # 调头
        # Element_Table = [77, 0]  # 边界参数调头     # 都用边界参数的掉头了

        # Element_Table = [9, 12, 0]       # # 右小 + 交叉倒车（右——左）
        # Element_Table = [8, 13, 0]       # # 左小 + 交叉倒车（左——右）
        # Element_Table = [8, 15, 0]       # # 左小 + 交叉正车（左——右）
        # Element_Table = [9, 14, 0]       # # 右小 + 交叉正车（右——左）

        # Element_Table = [9, 10, 0]        # 右小 + 从左往右扫描，普通正车
        # Element_Table = [8, 17, 0]        #左小 + 从右往左扫描，普通正车
        # Element_Table = [8, 2, 0]         #左小 + 普通倒车入库--从左往右扫描
        # Element_Table = [9, 16, 0]        # 右小 + 普通倒车入库--从右往左扫描

        # Element_Table = [6, 66, 444, 0]  # 直行+边界参数左大
        # Element_Table = [6, 66, 555, 0]  # 直行+边界参数右大
        # 下面的随机跑图测试需要放到右边外道的起点，正向
        # Element_Table = [3, 6, 66, 4, 6, 66, 9, 11, 0]      # 随机跑图测试0,包含侧方
        # Element_Table = [6, 66, 7, 6, 66, 9, 9, 12, 0]      # 随机跑图测试1，包含交叉倒车（右——左）
        # Element_Table = [6, 66, 7, 9, 14, 0]      # 随机跑图测试3，包含交叉正车（右——左）

        # 下面的测试跑图测试需要放到右边外道的起点，逆向
        # Element_Table = [5, 8, 13, 0]      # 随机跑图测试2，包含交叉倒车（左——右）
        # Element_Table = [6, 66, 5, 15, 0]            # 随机跑图测试4，交叉正车（左——右）

        # 测试去年比赛2023跑图
        # Element_Table = [6, 66, 1, 6, 66, 4, 14, 0]         # 去年第一轮 3:15
        # Element_Table = [1, 9, 6, 66, 444, 4, 9, 99, 9, 10, 0]      # 去年第二轮 3:40
        # Element_Table = [4, 4, 99, 6, 66, 11, 0]      # 去年第三轮 2:38

        # 测试今年的2024规则上的演示图
        # Element_Table = [6, 66, 6, 66, 1, 4, 9, 16, 0]   # 第一张图
        # Element_Table = [6, 66, 4, 6, 66, 99, 6, 66, 9, 10, 0]   # 第二张图

        # 资源群里面的示例图
        # Element_Table = [6, 66, 1, 4, 6, 66, 77, 4, 16, 0]  # 第一张图

        ####################
        #########元素稳定性测试
        ####################
        # 左大和边界左大测试
        # 左大，左大，右小，直走，右小，直走，边界左大，直走（可以循环了）测试左大和边界左大
        # Element_Table = [4, 4, 9, 6, 66, 9, 6, 66, 444, 6, 66, 0]  
        # Element_Table = [4, 4, 9, 6, 66, 9, 6, 66, 444, 6, 66, 4, 4, 9, 6, 66, 9, 6, 66, 444, 6, 66, 0]  #重复两次

        # 右大和边界右大测试
        # 右大，左小，边界右大，右大，直走，边界右大（重复）
        # Element_Table = [5, 8, 555, 5, 6, 66, 555, 0]  #重复一次就行了  

        # 右小测试
        # 调头，右小，右小（一直右小就行了）
        # Element_Table = [77, 9, 9, 9, 9, 9, 9, 0]  #重复一次就行了 

        # 边界调头测试
        # 左大，调头，调头（重复）
        # Element_Table = [4, 77, 77, 4, 77, 77, 4, 77, 77, 0]   #重复三次

        # 内外道切换调头测试
        # 调头， 直走，调头，直走（重复）
        # Element_Table = [77, 6, 66, 77, 6, 66, 77, 6, 66, 77, 6, 66, 77, 6, 66, 77, 6, 66, 0]   #重复三次

        # 直走测试(包括，边界直走，中心直走，无线直走)
        # 直走，直走，直走，左大，直走，右小，直走，直走，调头(重复）
        # Element_Table = [6, 66, 6, 66, 6, 66, 4, 6, 66, 99, 6, 66, 6, 66, 77, 6, 66, 6, 66, 6, 66, 4, 6, 66, 99, 6, 66, 6, 66, 77, 0]   #重复两次

        # 避障测试（包括内外道)
        # 避障，调头，避障，调头（重复）(用的都是内道避障模式)
        # Element_Table = [1, 77, 1, 77, 1, 77, 1, 77, 1, 77, 1, 77,0]  #重复三次

        # 正车和倒车以及侧方入库部分测试（在上面模板里有，一共八种正倒车情况，外加两种侧方）重点测试侧方三遍起步

        pass

    print('Element_Table:{}'.format(Element_Table))
    # while True:
    #     pass    # 无限阻塞，查看是否外部修改参数成功
    # 状态表的默认索引
    Element_Table_Index = 0
    Element_Table_last_Index = Element_Table_Index

    # 倒车时间控制
    fix_time = 4

    # 小车的校0角度
    angle_zero = -3

    # 蓝线检测积分，初始值为0
    blue_line_score = 0

    # 巡线图片预处理初始化代码
    Capture_lane_image.init_lane_detect()
    HSV_Capture_lane_image.init_lane_detect()

    # 元素视觉部分初始化代码
    first_detect.init_detect_portable()

    # 巡线部分初始化代码
    wig_lane_dec.init_lane_detect()

    # 蓝线检测部分的初始化代码
    blue_line.init_detect()
    stop_blue.init_detect()

    # 侧方视觉辅助部分的初始化代码----------
    cefang_Longitudinal_flag = 0
    long_state_flag = 0
    points_1 = (0, 0)
    points_2 = (0, 0)
    H_cefang_line = None
    H_F_slope = None
    cefang_hengxiang_flag = 0
    x_error = 0
    cefang_execute_flag = 0
    picture_count = 0
    cefang.cefang_line_init()

    # 侧方白线的条数，选择侧方停车的格子，因此初始化参数很重要
    # 0---第一个格子
    # 1---第二个格子
    # 2---第三个格子
    white_lines_num = 1

    # 对位置选择的一种记录，用于最后横向矫正的相关动作选择
    white_lines_num_record = white_lines_num

    # 侧方位置选择的步骤标志
    set_location_flag = 0

    # 侧方开始的计数器
    cefang_start_count = 0

    # 交叉入库的相关变量
    jiaocha_ruku_H_line_detect_flag = 0
    # 竖线检测出来的数据参数
    jiaocha_ruku_H_line = None
    # 竖线检测时候的卷积次数全局设定
    jiaocha_ruku_H_line_iterations = 5
    # 交叉入库卷积核的设定
    kernel_set_jiaocha_ruku_line = cv2.getStructuringElement(shape=cv2.MORPH_RECT,
                                                             ksize=(3, 3),
                                                             anchor=(-1, -1)
                                                             )

    # 交叉入库斜率巡线的参数
    jiaocha_ruku_line = None
    jiaocha_ruku_H_slope = None

    # 交叉入库的执行标志位
    JC_daoche_ruku_Execute_flag = 0

    # 动作组执行标志位1
    action_l_t_r = 0
    # 动作组执行标志位2
    action_r_t_l = 0
    # 动作组直行标志位3
    action_l_t_r_front = 0
    # 动作组直行标志位4
    action_r_t_l_front = 0

    # 交叉入库用到的控制阶段标志位
    # 倒车（右——左）
    jc_ruku_rtl_back_flag = 0
    # 倒车（左——右）
    jc_ruku_ltr_back_flag = 0
    # 正车（右——左）
    jc_ruku_rtl_front_flag = 0
    # 正车（左——右）
    jc_ruku_ltr_front_flag = 0

    # 内道避障的执行标志位
    bizhang_nei_exe_flag = 0

    # 内道所使用的全局时间
    nei_start_time = 0


# --------------------------------------------------------------------
# 巡线所使用的执行函数
# 暂时没有调用
# 正常模式的巡线(有延迟)
def lane_detect_normal(img):
    # 对图片进行处理
    bina_img = Capture_lane_image.get_lane_image(img)

    # 进行边界的检测
    left_bound_445, right_bound_445 = wig_lane_dec.boundary_detect(bina_img)

    # 进行巡线模式的检测
    dec_line_mode = wig_lane_dec.get_line_mode(left_bound_445, right_bound_445)

    # 给出巡线的角度和倒车标志
    angle, pos_flag = wig_lane_dec.get_angle(left_bound_445, right_bound_445, dec_line_mode, bina_img)

    print('angle_original:', angle)

    # 角度的固定调0
    steering_angle = angle + angle_zero

    print('angle:', steering_angle)

    # -------------------------------------------------------------------
    cv2.imshow('bina_img', bina_img)

    # 等待10ms看有没有按键按下，读取指令
    key_wig = cv2.waitKey(1)
    # ---------------------------------------------------------------------

    return steering_angle, pos_flag


# 强制左边线巡线模式
def lane_detect_find_left_line(img):
    # 对图片进行处理
    bina_img = Capture_lane_image.get_lane_image(img)

    # 进行边界的检测
    left_bound_445, right_bound_445 = wig_lane_dec.boundary_detect(bina_img)

    # 强制设置巡线模式为特殊模式5
    dec_line_mode = 5

    # 给出巡线的角度和倒车标志
    angle, pos_flag = wig_lane_dec.get_angle(left_bound_445, right_bound_445, dec_line_mode, bina_img)

    print('angle_left_original:', angle)

    # 角度的固定调0
    steering_angle = angle + angle_zero

    print('angle_left:', steering_angle)

    # -------------------------------------------------------------------
    cv2.imshow('bina_img', bina_img)

    # 等待10ms看有没有按键按下，读取指令
    key_wig = cv2.waitKey(1)
    # ---------------------------------------------------------------------

    return steering_angle, pos_flag


# 强制右边线巡线模式
def lane_detect_find_right_line(img):
    # 对图片进行处理
    bina_img = Capture_lane_image.get_lane_image(img)
    # 进行边界的检测
    left_bound_445, right_bound_445 = wig_lane_dec.boundary_detect(bina_img)
    # 强制设置巡线模式为特殊模式6
    dec_line_mode = 6
    # 给出巡线的角度和倒车标志
    angle, pos_flag = wig_lane_dec.get_angle(left_bound_445, right_bound_445, dec_line_mode, bina_img)

    print('angle_right_original:', angle)

    # 角度的固定调0
    steering_angle = angle + angle_zero

    print('angle_right:', steering_angle)

    # -------------------------------------------------------------------
    cv2.imshow('bina_img', bina_img)

    # 等待10ms看有没有按键按下，读取指令
    key_wig = cv2.waitKey(1)
    # ---------------------------------------------------------------------

    return steering_angle, pos_flag


# ---------------------------------------------------------------------
# 判断函数的定义

# 基本的判断函数
# 蓝线检测和控制的相关函数函数
# --------------------
# 通过蓝线中点进行距离修正的函数
def fix_by_blue_mid_points(points1, points2, lines):
    # # 进行一次透视变换，但是不对图像进行裁剪
    # img = Capture_lane_image.warp_image(img)
    #
    # # 经过一番考虑，还是进行图像切割
    # # 先将图像进行裁剪，只要下半部分
    # img = img[240:, :]

    # 还是先进行蓝线检测
    # points1, points2, line = stop_blue.find_blue_line_give_points(img)

    # 由于是在内部使用，图像预处理的部分暂时不用

    # 得到中线距离
    mid_points_y = stop_blue.mid_points_distance(points1, points2)

    print('Blue_line_mid_points_y:', mid_points_y)

    if lines is not None:
        print('FFFFFFFIIIIINNNNDDDD_mid_points_blue_line')

        if mid_points_y > 110:
            print("blue_line_back")
            msg.drive.steering_angle = -6
            msg.drive.speed = 15 * dianji_fuhao
            pub.publish(msg)
            rospy.sleep(1)
            return 1
        # 这里做了数据更正，就是里线太远的时候，向前走0.5
        if mid_points_y < 60:
            print("blue_line_forwarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrd")
            msg.drive.steering_angle = -6
            msg.drive.speed = -15 * dianji_fuhao
            pub.publish(msg)
            rospy.sleep(1)
            return 1

        else:
            print('perfect')
            msg.drive.steering_angle = -6
            msg.drive.speed = 0
            pub.publish(msg)
            return 1

    else:
        print('Not find_MMMIIIIDDD_points Blue_line')
        msg.drive.steering_angle = -6
        msg.drive.speed = 0
        pub.publish(msg)
        return 0


# 蓝线检测函数
# 检测蓝线之后给出一个指令，可以根据指令1，来进行相关的基本控制
def stop_by_blue_line(img):
    # 蓝线检测的积分
    global blue_line_score

    # 在摄像头位置修正之后，采用透视变换
    img = Capture_lane_image.warp_image(img)

    # 先将图像进行裁剪，只要下半部分
    stop_line_img = img[240:, :]

    # 进行基本的蓝线检测
    points1, points2, line = stop_blue.find_blue_line_give_points(stop_line_img)

    # 绘制蓝线的图
    line_img = stop_blue.draw_line_2(stop_line_img, line)

    # 搞到蓝线的斜率
    angle_blue = stop_blue.get_slope(points1, points2)

    # 打印得到的点
    print("points:", points1, points2)

    # 显示原图和蓝线绘制图
    cv2.imshow('original', stop_line_img)
    cv2.imshow('first_stop_by_blue_line:', line_img)

    # 等待10ms看有没有按键按下，读取指令
    key_wig = cv2.waitKey(1)

    # 如果有蓝线，并且检测到斜率
    if -15 < angle_blue < 15:
        print('Blue_line_core_improve_____________!!!!!!!!!!!!!!!!!!!!!')
        blue_line_score += 1
    else:
        print('stop_line_not_find')

    # 蓝色积分为3，那就认定为有蓝色的线
    if blue_line_score >= 3:
        print('Stop by blue line')

        # 在第一次蓝线检测里面发出停止的指令
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        # rospy.sleep(1)    # 为了时间这里的sleep删除
        print("sleep 1s and fix by mid points")

        # 进行蓝线距离的修正
        mid_points_flag = fix_by_blue_mid_points(points1, points2, line)
        if mid_points_flag == 1:

            return 1

        else:
            print('+++++++++++++++++++++++++++++++Mid_fix_process is wrong')
            return 1

    else:
        return 0


# 检测出蓝线之后给出矫正数据
def fix_by_blue_line(img):
    global fix_time  # 执行蓝线修正的时间
    print('fix_by_blue_line')

    # 进行一次透视变换，但是不对图像进行裁剪

    img = Capture_lane_image.warp_image(img)

    # 经过一番考虑，还是进行图像切割

    # 先将图像进行裁剪，只要下半部分
    img = img[240:, :]

    # 现在是停止的阶段，直接进行检测
    points1, points2, line = stop_blue.find_blue_line_give_points(img)

    # 绘制蓝线的图
    # line_img = stop_blue.draw_line_2(img, line)
    # cv2.imshow('fix_line_img:', line_img)
    # cv2.waitKey(1)

    # 搞到蓝线的斜率
    # 这里如果没有检测到蓝线，就会返回None出去
    angle_blue = stop_blue.get_slope(points1, points2)

    # 进行角度修正的时间
    # 这里如果不加上检验会报个错，但是不影响程序运行
    if angle_blue is None:
        print('!!!!!!!!!!!!!!!!angle_blue_is_None___________________Can not find FIX blue line')
        pass
    else:
        # 计算修正时间的算法，可能需要随视野和蓝线检测进行调整
        fix_time = abs(angle_blue / 4)

    # fix_time = 3

    # 斜率的转为角度的修正系数
    K = 4
    if angle_blue is None:
        print('!!!!!!!!!!!!!!!!angle_blue_is_None___________________Can not find FIX blue line')
        pass
    else:
        angle_blue = angle_blue * K
        print('WOOOOOOO!!!!!!!,Find the FIX Blue line!!!!!!!!!!_______________________________')

    print('angle_blue_fix_1:', angle_blue)
    print('fix_time:', fix_time)
    # 给点时间你来看调整的数据
    # print('sleep 2s')   # 为了时间，这里的sleep删除
    # rospy.sleep(2)

    # 倒车的速率
    speed_fix = 15

    # 这里如果没有检测到蓝线，就会返回None出去
    return angle_blue, speed_fix


# 蓝线校准控制代码
def fix_blue_line_control(img):
    # 角度矫正的持续时间
    global fix_time

    print('enter_fix_by_blue_line_control')

    # 根据蓝线静止的检测获得的角度，并给出速度
    angle, speed_abs = fix_by_blue_line(img)

    if angle is not None:
        # 后退修正角度
        msg.drive.speed = speed_abs * dianji_fuhao
        msg.drive.steering_angle = angle + angle_zero
        # 进行发布的操作
        pub.publish(msg)
        # 修正操作的时长
        rospy.sleep(fix_time)

        if angle > 0:
            msg.drive.steering_angle = -6  # 这里先不用宏定义的速度-3
        elif angle < 0:
            msg.drive.steering_angle = 0
        else:
            msg.drive.steering_angle = 0
        # 前进回正位置
        msg.drive.speed = speed_abs * (-1) * dianji_fuhao

        # 进行发布的操作
        pub.publish(msg)
        # 回正操作的时长
        rospy.sleep(fix_time)

        return 1

    else:
        print('fix_line_not_find')
        return 0


# ------------------------------------------------------------------------
# 侧方基本控制函数

# 下面执行侧方停车的矫正
# -----------------------------------

# 用于过渡图片的等待函数（测试）,用于测试图片回调程序的
def Throw_picture_wait(img, time_s):
    # 图片序列等待长度
    global picture_count

    # 打印等待序号
    print('picture_count_time:', picture_count)

    # 计算对应等待的时间
    count_set = time_s * 10

    # 通过不执行来过度序列
    if picture_count <= count_set:
        # 执行一次增加一次
        picture_count += 1
        return 0
    else:
        # 执行完成之后，对标志进行清零
        picture_count = 0
        return 1


# 用于平行修正的执行函数
# 平行向左（小）
def pingxing_fix_left_small(left_back_time, right_back_time, straight_fix_time):
    # 左后指令
    msg.drive.speed = 15 * dianji_fuhao
    msg.drive.steering_angle = -20
    pub.publish(msg)
    print('left_back')
    rospy.sleep(left_back_time)

    # 右后指令
    msg.drive.speed = 15 * dianji_fuhao
    msg.drive.steering_angle = 20
    pub.publish(msg)
    print('right_back')
    rospy.sleep(right_back_time)

    # 向前回正指令
    msg.drive.speed = -15 * dianji_fuhao
    msg.drive.steering_angle = ((-8) / straight_fix_time) - 4
    pub.publish(msg)
    print('straight_fix')
    rospy.sleep(straight_fix_time)

    # 停止指令
    msg.drive.speed = 0
    msg.drive.steering_angle = 0
    pub.publish(msg)


# 平行向右（小）
def pingxing_fix_right_small(right_back_time, left_back_time, straight_fix_time):
    # 右后指令
    msg.drive.speed = 15 * dianji_fuhao
    msg.drive.steering_angle = 20
    pub.publish(msg)
    print('right_back')
    rospy.sleep(right_back_time)

    # 左后指令
    msg.drive.speed = 15 * dianji_fuhao
    msg.drive.steering_angle = -20
    pub.publish(msg)
    print('left_back')
    rospy.sleep(left_back_time)

    # 向前回正指令
    msg.drive.speed = -15 * dianji_fuhao
    msg.drive.steering_angle = 4 / straight_fix_time
    pub.publish(msg)
    print('straight_fix')
    rospy.sleep(straight_fix_time)

    # 停止指令
    msg.drive.speed = 0
    msg.drive.steering_angle = 0
    pub.publish(msg)


# 平行向左（小） 前-后
def pingxing_fix_left_small_front(left_front_time, right_front_time, straight_fix_time):
    # 左前指令
    msg.drive.speed = -15 * dianji_fuhao
    msg.drive.steering_angle = -20
    pub.publish(msg)
    print('left_back')
    rospy.sleep(left_front_time)

    # 右前指令
    msg.drive.speed = -15 * dianji_fuhao
    msg.drive.steering_angle = 20
    pub.publish(msg)
    print('right_back')
    rospy.sleep(right_front_time)

    # 向后回正指令
    msg.drive.speed = 15 * dianji_fuhao
    msg.drive.steering_angle = ((-8) / straight_fix_time) - 4
    pub.publish(msg)
    print('straight_fix')
    rospy.sleep(straight_fix_time)

    # 停止指令
    msg.drive.speed = 0
    msg.drive.steering_angle = 0
    pub.publish(msg)


# 平行向右（小） 前-后
def pingxing_fix_right_small_front(right_front_time, left_front_time, straight_fix_time):
    # 右前指令
    msg.drive.speed = -15 * dianji_fuhao
    msg.drive.steering_angle = 20
    pub.publish(msg)
    print('right_front')
    rospy.sleep(right_front_time)

    # 左前指令
    msg.drive.speed = -15 * dianji_fuhao
    msg.drive.steering_angle = -20
    pub.publish(msg)
    print('left_front')
    rospy.sleep(left_front_time)

    # 向后回正指令
    msg.drive.speed = 15 * dianji_fuhao
    msg.drive.steering_angle = 4 / straight_fix_time
    pub.publish(msg)
    print('straight_fix')
    rospy.sleep(straight_fix_time)

    # 停止指令
    msg.drive.speed = 0
    msg.drive.steering_angle = 0
    pub.publish(msg)


# -----------------------------------------分块的步骤执行函数
# 下面是根据白色横线进行矫正的函数
def Back_by_hengxian_control(img):
    # 首先直接进行图像的预处理
    # 直接获得侧方专用的二值化图片
    cefang_bina_image = Capture_lane_image.cefang_image(img)

    # 对图像进行裁剪
    cefang_bina_image = cefang_bina_image[190:290, 190:450]
    # 显示一下裁剪之后的图像
    # cv2.imshow('cefang_bina_image:', cefang_bina_image)
    # cv2.waitKey(1)

    # 进行相关的横向直线检测，这里内部已经没有图像处理了
    points1, points2, W_cefang_line = cefang.W_line_detect_filter_give_points(cefang_bina_image)

    # -------------------------------------------------------------------
    # 进行图像的相关绘制-非必要
    # 绘制图像之前进行裁剪
    # draw_img = Capture_lane_image.cefang_warp_image(img)
    # draw_img = draw_img[190:290, 190:450]
    # # 显示裁剪之后的图像
    # cv2.imshow('cefang_original:', draw_img)
    #
    # # 进行横线的绘制
    # line_all_img = cefang.draw_line_first(draw_img, W_cefang_line)
    # cv2.imshow('hengxian:', line_all_img)
    #
    # cv2.waitKey(1)
    # -------------------------------------------------------------------

    # 判断是否在前面的一定区域检测到了白色的横线，检测到横线，就进行后退的操作
    if W_cefang_line is not None:
        # 这里进行后退的操作
        msg.drive.speed = 15 * dianji_fuhao
        msg.drive.steering_angle = -3
        pub.publish(msg)
        print('backbackback')
        rospy.sleep(0.4)
        return 0


    elif W_cefang_line is None:
        # 如果没有检测到白色的横线，就停车，并进入下一步操作
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        print('Finish_hengxian')
        cv2.destroyAllWindows()
        return 1

    pass


# 下面是进行根据竖直的线进行车头角度矫正的函数（当前的函数可以被调用多次）
# 由于在最后阶段，也进行了一次车头矫正，但是不需要附带的平移，所以这里导入最后阶段的标志位final_flag
def Longitudinal_line_fix_control(img, final_flag):
    # 引入全局的检测+执行标志
    global cefang_Longitudinal_flag

    # 侧方局部动作执行的一些标志
    global long_state_flag

    # 引入直线检测的一些参数
    global points_1, points_2, H_cefang_line, H_F_slope

    # 首先直接进行图像的预处理
    # 直接获得侧方专用的二值化图片
    cefang_bina_image = Capture_lane_image.cefang_image(img)

    # 这里进行统一的图像裁剪处理
    cefang_bina_image = cefang_bina_image[210:290, :]

    # 图片显示
    cv2.imshow('cefang_bina_Longitudinal:', cefang_bina_image)
    cv2.waitKey(1)
    # ----------------------------------------------------------------------------------

    # 停止阶段-特殊情况进入-1
    if cefang_Longitudinal_flag == -1:
        print('Finish CCCCCCCCCCCCe__________________________FFFFFFFFFFFFFFFFang_fix')
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        # 为了能多次使用这些函数，目前将标志位清零
        cefang_Longitudinal_flag = 0
        return 1

    # 接下来是侧方的直线检测阶段
    # 进入阶段0
    if cefang_Longitudinal_flag == 0:

        # 进入到阶段1，再进行直线的检测
        # 接下来进行相关数据的检测
        # 进行相关的纵向直线斜率检测，这里内部的图像处理已经去除
        # 这里增加了一个是否检测结束的标志位
        points_1, points_2, H_cefang_line, H_F_slope, H_Finish_flag = cefang.H_line_detect_filter_give_points(
            cefang_bina_image)

        # 首先判断扫描进程是否结束
        if H_Finish_flag == 1:
            print('H_F_finish_flag is 1')
            # 先检测直线是否为空
            # 如果直线检测不为空
            if H_cefang_line is not None:
                print('find_H_line')
                print('slope: ', H_F_slope)
                # 检测的结果已经比较垂直了，则跳过矫正
                if abs(H_F_slope) > 89:
                    msg.drive.speed = 0
                    msg.drive.steering_angle = 0
                    pub.publish(msg)
                    print('Slope is >>>>> 80,________999999   000000____________finiiiishsleep2')
                    cefang_Longitudinal_flag = -1
                    return 0

                # 如果检测的直线没有很垂直，就进入下一个阶段进行调整
                else:
                    # 进入下一个阶段
                    cefang_Longitudinal_flag = 1

                    # 设置好下一个阶段的标志位啦
                    long_state_flag = 0
                    return 0

            # 如果检测的直线为空，就进行倒退
            else:
                print('Not find_H_line')
                msg.drive.speed = 15 * dianji_fuhao
                msg.drive.steering_angle = -3
                pub.publish(msg)
                print('backbackback')
                rospy.sleep(0.25)
                return 0


        # 如果扫描的进程没有结束，就什么都不做
        else:
            print('H_F_finish_flag is 00000')
            msg.drive.speed = 0
            msg.drive.steering_angle = -3
            pub.publish(msg)
            return 0
            pass

    # 接下来进入侧方的调整执行阶段
    # 进入阶段1
    if cefang_Longitudinal_flag == 1:
        # 进行竖直角度矫正

        # 如果输入角度是大于0的竖的白线
        # 角度向右调整
        if H_F_slope > 0:

            # 局部操作的第一步进行角度调整，之后还有平行方向上的附带修正
            if long_state_flag == 0:
                # 计算调整的角度，和对应的调整时间
                print('slope > 0')
                slope_error = 90 - H_F_slope
                print('sleep_error: ', slope_error)
                cefang_time = slope_error / 6
                angle = slope_error * 2

                if cefang_time < 1.5:
                    # 设置最小的执行时间
                    cefang_time = 1.5

                if angle < 6:
                    # 设置最小执行角度
                    angle = 6

                print('cefang_time:', cefang_time)
                print('angle_slope_error:', angle)

                # 左后指令
                msg.drive.speed = 15 * dianji_fuhao
                msg.drive.steering_angle = -angle - 3
                pub.publish(msg)
                print('left_back')
                rospy.sleep(cefang_time)

                # 右前指令
                msg.drive.speed = -15 * dianji_fuhao
                msg.drive.steering_angle = angle
                pub.publish(msg)
                print('right_front')
                rospy.sleep(cefang_time)

                # 停止指令
                msg.drive.speed = 0
                msg.drive.steering_angle = 0
                pub.publish(msg)
                rospy.sleep(1)

                # 进入调整的下一步附带的平行修正
                long_state_flag = 1
                return 0

            # 第二步附带的平行修正第一次
            # 经过考虑，第一次平行修正放弃
            # if long_state_flag == 1:
            #     # 直接进行一次向左的矫正
            #     pingxing_fix_left_small(left_back_time=3,
            #                             right_back_time=3,
            #                             straight_fix_time=6
            #                             )
            #
            #     # 进入下一次平行矫正
            #     long_state_flag = 2
            #     return 0

            # 第二步附带的平行修正第二次
            if long_state_flag == 1:

                if final_flag == 0:
                    # 直接进行一次向左的矫正
                    pingxing_fix_left_small(left_back_time=2.5,
                                            right_back_time=2,
                                            straight_fix_time=5
                                            )
                # 如果是最后的修正阶段，那么就不进行左右的修正了
                else:
                    pass
                # 结束调整
                long_state_flag = 0
                cefang_Longitudinal_flag = -1
                return 0

        # 如果输入角度是小于0的竖的白线
        # 角度向左调整
        elif H_F_slope < 0:

            # 局部操作的第一步进行角度调整，之后还有平行方向上的附带修正
            if long_state_flag == 0:

                # 计算调整的角度，和对应的调整时间
                print('slope < 0')
                slope_error = 90 + H_F_slope
                print('sleep_error: ', slope_error)
                cefang_time = slope_error / 6
                angle = slope_error * 2

                if cefang_time < 1.5:
                    # 设置最小的执行时间
                    cefang_time = 1.5

                if angle < 6:
                    # 设置最小执行角度
                    angle = 6

                print('cefang_time:', cefang_time)
                print('angle_slope_error:', angle)

                # 右后指令
                msg.drive.speed = 15 * dianji_fuhao
                msg.drive.steering_angle = angle
                pub.publish(msg)
                print('right_back')
                rospy.sleep(cefang_time)

                # 左前指令
                msg.drive.speed = -15 * dianji_fuhao
                msg.drive.steering_angle = -angle - 3
                pub.publish(msg)
                print('left_front')
                rospy.sleep(cefang_time)

                # 停止指令
                msg.drive.speed = 0
                msg.drive.steering_angle = 0
                pub.publish(msg)
                rospy.sleep(1)

                # 进入调整的下一步附带的平行修正
                long_state_flag = 1
                return 0

            # 第二步附带的平行修正
            # 经过考虑，第一次平行修正放弃
            # if long_state_flag == 1:
            #     # 直接进行一次向右的矫正
            #     pingxing_fix_right_small(right_back_time=3,
            #                              left_back_time=3,
            #                              straight_fix_time=6
            #                              )
            #
            #     # 进入下一次平行矫正
            #     long_state_flag = 2
            #     return 0

            # 第二步附带的平行修正第二次
            if long_state_flag == 1:

                if final_flag == 0:
                    # 直接进行一次向右的矫正
                    pingxing_fix_right_small(right_back_time=2.5,
                                             left_back_time=2.5,
                                             straight_fix_time=5
                                             )
                # 如果是最后的修正阶段，那么就不进行左右的修正了
                else:
                    pass
                # 结束调整
                long_state_flag = 0
                cefang_Longitudinal_flag = -1
                return 0


# 用于进行根据侧方停车竖直线段进行横向矫正的函数（当前的函数可以被调用多次）
# 这个函数使用了两次，一次是开头修正，一次是最后的修正，但是两次的修正的前进参数不同，为了解耦，调用了final_flag，如果final_flag = 1
# 说明是最后的修正，两边的修正操作的参数不同
# 这里最后还是舍弃了final_flag,改成了front_flag
def Longitudinal_line_hengxiang_fix_control(img, front_flag):
    # 横向矫正的步骤标志
    global cefang_hengxiang_flag
    # x方向上的误差参数
    global x_error
    # 首先直接进行图像的预处理
    # 直接获得侧方专用的二值化图片
    cefang_bina_image = Capture_lane_image.cefang_image(img)

    # 这里进行统一的图像裁剪处理
    cefang_bina_image = cefang_bina_image[210:290, :]

    # 进入循环扫描的中止条件，就进入这个状态
    if cefang_hengxiang_flag == -1:
        print('finish hengxiang_finish_stop_3_picture_time')
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        # 为了能多次调用这个函数，因此将标志位清零
        cefang_hengxiang_flag = 0
        return 1

    # 第一阶段 参数的扫描阶段
    if cefang_hengxiang_flag == 0:

        # 进行相关的纵向边界检测
        left_bound_cefang_255, right_bound_cefang_255 = cefang.cefang_get_boundary(cefang_bina_image)

        # 如果检测不到直线，返回的就是None
        x_error = cefang.Longitudinal_result_action_judge_num(left_bound_cefang_255, right_bound_cefang_255,
                                                              cefang_bina_image)

        # 检测一下，是否检测到了直线边界嘞
        if x_error is not None:
            # 乘以一个系数
            # x_error = float(x_error) * 0.8

            # 进行图片的显示
            print('x_error: ', x_error)
            # 图片显示
            cv2.imshow('cefang_bina_Longitudinal:', cefang_bina_image)
            cv2.waitKey(1)

            # 如果符合了停止条件
            if abs(x_error) <= 32:
                # 就进入横向调整的结束阶段
                cefang_hengxiang_flag = -1
                return 0

            # 否则就进入下一个调整阶段
            else:
                # 进入下一个修正阶段
                cefang_hengxiang_flag = 1
                return 0

        # 如果没有检测到直线的边界,就倒退一会
        else:
            print('Not find_boundary_line')
            msg.drive.speed = 15 * dianji_fuhao
            msg.drive.steering_angle = -3
            pub.publish(msg)
            print('backbackback')
            rospy.sleep(0.25)
            return 0

    # -------------------------------------------------------------下面是执行矫正的控制
    if cefang_hengxiang_flag == 1:

        # 直接进行修正

        # 向右修正
        if x_error > 0:
            print('x_error > 0')
            # 右修正函数
            if front_flag == 1:
                # 向前修正的函数
                pingxing_fix_right_small_front(right_front_time=2,
                                               left_front_time=2,
                                               straight_fix_time=4)

            else:
                # 向后修正的函数
                pingxing_fix_right_small(right_back_time=2,
                                         left_back_time=2,
                                         straight_fix_time=4)

            # 回到第一个阶段去进行扫描
            cefang_hengxiang_flag = 0
            return 0

        # 向左修正
        elif x_error < 0:
            print('x_error < 0')
            # 左修正函数

            if front_flag == 1:
                # 向前修正
                pingxing_fix_left_small_front(left_front_time=2,
                                              right_front_time=2,
                                              straight_fix_time=4)

            else:
                # 倒退修正
                pingxing_fix_left_small(left_back_time=2,
                                        right_back_time=2,
                                        straight_fix_time=4)

            # 回到第一个阶段去进行扫描
            cefang_hengxiang_flag = 0
            return 0

        # 出错了也回到上一个阶段去扫描
        else:
            print('the x_error is wrong')
            # 回到第一个阶段去进行扫描
            cefang_hengxiang_flag = 0
            return 0


# 用于执行后半部分倒车到指定位置的函数（第一条横线需要避开），所以需要搭配Back_by_hengxian_control使用
def cefang_set_123_location(img):
    # 侧方选择位置的标志参数
    global set_location_flag
    # 白线的条数(最大为2)
    global white_lines_num

    # 引入直线检测的一些参数
    # 后面选择使用霍夫线斜率检测的方案进行巡线，可能存在很大的延迟
    global points_1, points_2, H_cefang_line, H_F_slope

    # 首先判断是否需要进行位置调整
    # 如果白线条数为0，则说明已经不需要进行位置调整处理了
    if white_lines_num == 0:
        print('white_lines_num_is 0')
        msg.drive.steering_angle = 0
        msg.drive.speed = 0
        pub.publish(msg)
        return 1

    # 否则开始进行图像扫描，进入特殊的巡线
    else:
        # 打印当前的位置选择阶段
        print('set_location_flag: ', set_location_flag)
        print('white_lines_num_is:', white_lines_num)
        # 第一阶段，巡线
        # 同时扫描白色横线进行判断处理
        if set_location_flag == 0:

            # 首先直接进行图像的预处理
            # 直接获得侧方专用的二值化图片
            cefang_bina_image = Capture_lane_image.cefang_image(img)

            # 这里进行统一的竖线图像裁剪处理
            cefang_bina_image = cefang_bina_image[210:290, :]

            # 显示图片（调试）结果就是图像不更新了
            # cv2.imshow('set_location_img: ', cefang_bina_image)
            # cv2.waitKey(1)

            points_1, points_2, H_cefang_line, H_F_slope, H_F_flag = cefang.H_line_detect_filter_give_points(
                cefang_bina_image)

            # 先判断扫线是否结束
            if H_F_flag == 1:
                print('H_F_finish_flag is 1')
                # 如果是正常扫线，没有扫描到横线
                if H_cefang_line is not None:
                    print('H_F_cefang_line is normal')

                    if H_F_slope > 0:

                        slope_error = 90 - H_F_slope

                        angle_cefang = slope_error * 2

                        print('angle_set_location: ', angle_cefang)

                        # 左后巡线
                        # 直接给出巡线角度
                        msg.drive.steering_angle = -angle_cefang
                        # 给出巡线的速度（倒车行驶）
                        msg.drive.speed = 20 * dianji_fuhao
                        # 发布消息
                        pub.publish(msg)
                        return 0

                    elif H_F_slope < 0:

                        slope_error = 90 + H_F_slope

                        angle_cefang = slope_error * 2

                        print('angle_set_location: ', angle_cefang)

                        # 右后巡线
                        # 直接给出巡线角度
                        msg.drive.steering_angle = angle_cefang - 3
                        # 给出巡线的速度（倒车行驶）
                        msg.drive.speed = 20 * dianji_fuhao
                        # 发布消息
                        pub.publish(msg)
                        return 0


                else:
                    print('H_F_cefang_line is NNNNNNNone')
                    # 向后直线缓慢后退
                    msg.drive.steering_angle = -6
                    msg.drive.speed = 15 * dianji_fuhao
                    pub.publish(msg)

                    # 找横线
                    cefang_bina_image_W = Capture_lane_image.cefang_image(img)

                    # 对图像进行裁剪
                    cefang_bina_image_W = cefang_bina_image_W[190:290, 190:450]

                    # 进行相关的横向直线检测，这里内部已经没有图像处理了
                    points1_W, points2_W, WW_cefang_line = cefang.W_line_detect_filter_give_points(cefang_bina_image_W)

                    if WW_cefang_line is not None:
                        print('get_WWWWW_line in H_F_line_None')
                        # 停下来
                        msg.drive.steering_angle = -6
                        msg.drive.speed = 0
                        pub.publish(msg)
                        # 进入下一个阶段
                        set_location_flag = 1

                    return 0

            # 如果线段扫描还没有结束
            else:
                print('H_F_finish_flag is 0')
                # 啥都不管
                return 0

        # 第二阶段，扫描白色横线并倒车,直到无法扫描到白色横线
        elif set_location_flag == 1:
            # 根据白线执行一次白线倒车
            back_hengxian_flag = Back_by_hengxian_control(img)

            # 如果倒车流程结束了
            if back_hengxian_flag == 1:

                # 白线的计数器减一
                print('white_line_down')
                white_lines_num = white_lines_num - 1
                # 回到局部的第一阶段，巡线
                set_location_flag = 0
                return 0
            else:
                return 0


# 执行侧方停车矫正流程的函数
def cefang_fix_Execute_control(img):
    # 侧方执行标志
    global cefang_execute_flag
    # 侧方白线的位置记录参数
    global white_lines_num_record

    # 无条件打印flag
    print('cefang_execute_flag:', cefang_execute_flag)

    # 定义矫正结束进入的阶段
    if cefang_execute_flag == -1:
        print('Finish cefang_Execute_fixxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx')
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        rospy.sleep(60)
        return 1

    # 第一阶段——首先检测白色的横线
    if cefang_execute_flag == 0:

        # 进行白色横线的矫正
        back_flag = Back_by_hengxian_control(img)

        if back_flag == 1:
            # 进入下一个侧方的下一个执行阶段
            cefang_execute_flag = 1

        else:
            cefang_execute_flag = 0
        return 0


    # 第五阶段———倒车位置的选择
    elif cefang_execute_flag == 1:
        # 进行侧方位置的选择
        location_flag = cefang_set_123_location(img)

        if location_flag == 1:
            # 进入最后的矫正阶段
            cefang_execute_flag = 2
        else:
            cefang_execute_flag = 1
        return 0


    # 第六阶段————倒车的最后矫正2，车身位置
    elif cefang_execute_flag == 2:

        # 如果选择第三个位置，那么矫正就会向前
        if white_lines_num_record == 2:

            hengxiang_flag2 = Longitudinal_line_hengxiang_fix_control(img, front_flag=1)
            # 矫正结束
            if hengxiang_flag2 == 1:
                # 进入结束阶段
                cefang_execute_flag = -1

            else:
                cefang_execute_flag = 2
            return 0

        # 如果最后选择的不是第三个位置，那么矫正就会向后
        else:
            hengxiang_flag2 = Longitudinal_line_hengxiang_fix_control(img, front_flag=0)

            # 矫正结束
            if hengxiang_flag2 == 1:
                # 进入结束阶段
                cefang_execute_flag = -1

            else:
                cefang_execute_flag = 2
            return 0


# --------------------------------------------------------------额外增加的交叉入库相关函数
# 下面是交叉倒车入库的函数部分

# 交叉倒车入库需要用到的一个特殊的斜率函数，就是能检测垂直斜率的函数，与正常的有点不用，在侧方模块里面粘出来的
# 通过端点得到斜率
def get_slope(point1, point2):
    if point1[0] > point2[0]:
        k = (float(point1[1]) - float(point2[1])) / (float(point1[0] - point2[0]))
        angle_blue = - math.atan(k) / np.pi * 180
        print('angle_cefang:', angle_blue)
        return angle_blue

    elif point2[0] > point1[0]:
        k = (float(point1[1]) - float(point2[1])) / (float(point1[0] - point2[0]))
        angle_blue = - math.atan(k) / np.pi * 180
        print('angle_cefang:', angle_blue)
        return angle_blue

    elif point2[0] == point1[0]:
        # print("slope is zero,angle_cefang_line return None")
        # angle_blue = None
        # return angle_blue

        # 这里就说明是90度了，情况特殊
        angle_blue = 90
        return angle_blue

    else:
        print('glope is wrong!!')
        return None


# 根据蓝色横线进行倒退的函数，这里使用了蓝线的模块，记得添加蓝线的相关依赖
def Back_blue_line(img):
    # 采用透视变换
    img = Capture_lane_image.warp_image(img)

    # 先将图像进行裁剪，只要下半部分
    back_blue_line_img = img[240:, :]

    # 进行基本的蓝线检测
    points1, points2, line = stop_blue.find_blue_line_give_points(back_blue_line_img)

    # 搞到蓝线的斜率
    angle_blue = stop_blue.get_slope(points1, points2)

    # 如果有蓝线，并且检测到斜率符合蓝线的要求
    if -15 < angle_blue < 15:
        print('Back_blue_line')

        # 那就进行常规的倒退操作
        msg.drive.speed = 15 * dianji_fuhao
        # 具体的蓝线角度给到车子
        msg.drive.steering_angle = angle_blue + angle_zero
        pub.publish(msg)

        return 0

    # 如果没有检测到蓝线，就直接停止
    else:
        msg.drive.speed = 0
        msg.drive.steering_angle = angle_zero
        pub.publish(msg)
        print('finish_back_blue_line')
        return 1


# 由于这里没有使用模块，就直接定义一个获得竖线斜率的函数，和侧方的基本一样，但是为了减少代码耦合程度
# 内道避障，我也用了这里的斜率巡线嘞，如果要改写的话，记得考虑这个问题
def jiaocha_ruku_H_line_detect(img):
    # 用于控制扫描竖线的标志位
    global jiaocha_ruku_H_line_detect_flag
    # 竖线检测出来的数据参数
    global jiaocha_ruku_H_line
    # 竖线检测时候的卷积次数全局设定
    global jiaocha_ruku_H_line_iterations

    # # 首先获得二值图像，用于扫描斜率，用于调试
    # jiaocha_ruku_bina_img = Capture_lane_image.get_lane_image(img)

    # 这里图像在外部进行统一的处理了，就不在内部进行处理了
    jiaocha_ruku_bina_img = img

    # 第一步
    # 获得图片扫描直线，得到斜率
    if jiaocha_ruku_H_line_detect_flag == 0:

        # 对获得的侧方专用二值化图片进行初步的裁剪
        # H_img = original_bina_image_H[210:290, :]
        # 图像统一处理，不进行内部裁剪
        H_img = jiaocha_ruku_bina_img
        # 先进行相关的腐蚀操作吧
        # ----------------------------------------------------------这里进行腐蚀运算的操作
        # 进行腐蚀运算操作
        H_jiaocharuku_erode_line_img = cv2.morphologyEx(src=H_img,
                                                        op=cv2.MORPH_ERODE,
                                                        kernel=kernel_set_jiaocha_ruku_line,
                                                        # 腐蚀默认采用的卷积核大小为3*3，默认腐蚀次数10
                                                        # 这样进行腐蚀之后，目前的最小检测角度为73
                                                        # 如果卷积次数为11，则最小检测角度为82
                                                        iterations=jiaocha_ruku_H_line_iterations)

        # 显示腐蚀之后的图片，调试的时候用到的，实际运行的时候，就显示一坨白屏
        cv2.imshow('jiaocha_HHHH_erode_cal:', H_jiaocharuku_erode_line_img)
        cv2.waitKey(1)
        # ------------------------------------------------------------------------------

        # 这里使用设定的阈值来检测纵向直线，由于是小单位使用，所以这里的一些K=0.9值，还有threshold=35累计阈值就不进行全局处理了
        jiaocha_ruku_H_line = cv2.HoughLinesP(image=H_jiaocharuku_erode_line_img,
                                              rho=1 * 0.9,
                                              theta=np.pi / 180 * 0.9,
                                              threshold=35,
                                              minLineLength=20,
                                              maxLineGap=30)

        # 如果成功检测到了直线
        if jiaocha_ruku_H_line is not None:

            # 进入下一步操作，不是进入回调，而是直接进行操作
            jiaocha_ruku_H_line_detect_flag = 1
            pass

        # 如果没有检测到直线那就直接返回空了
        else:
            print('None_HHHH_line')
            # 没有检测到直线的情况
            # 就返回一个空值
            # 结束操作了
            points_1_none = (0, 0)
            points_2_none = (0, 0)
            # 这里的blue仅仅是一个线的代指变量，不是蓝线的数据
            blue_line_none = None
            # 返回这次扫描进程结束的标志位
            finish_flag = 1
            return points_1_none, points_2_none, blue_line_none, None, finish_flag

    # 第二步，根据直线的斜率决定下一步做什么
    if jiaocha_ruku_H_line_detect_flag == 1:

        # 设置进行操作的线条的序号，默认为0即可，有多条线可以更改序号
        n = 0
        H_points_1 = (jiaocha_ruku_H_line[n][0][0], jiaocha_ruku_H_line[n][0][1])
        H_points_2 = (jiaocha_ruku_H_line[n][0][2], jiaocha_ruku_H_line[n][0][3])

        # 直接获得第一条检测到的直线的斜率
        H_slope = get_slope(H_points_1, H_points_2)

        # 判断检测的斜率是否为None
        if H_slope is not None:

            # 根据斜率进行检测到的直线筛选
            # 角度需要大于70是一定的
            if abs(H_slope) >= 10:
                print('FFFFFFFFFFFFFFFind_Longitudinal_HHHHHHHHHHHHHHHH_line')

                # 在内部再进行一次角度细分
                # 如果满足倾角较为接近90的的话，那就再进行一次细致的扫描，降低误差
                if abs(H_slope) > 82 and jiaocha_ruku_H_line_iterations != 6:
                    print('H_slope is > 82 ,enter next scan')

                    # 将卷积的次数设置为6
                    jiaocha_ruku_H_line_iterations = 6
                    # 回到第一步
                    jiaocha_ruku_H_line_detect_flag = 0

                    # 这次扫描就返回一个空值
                    points_1_none = (0, 0)
                    points_2_none = (0, 0)
                    blue_line_none = None
                    # 就返回扫描进程尚未结束
                    finish_flag = 0

                    return points_1_none, points_2_none, blue_line_none, None, finish_flag


                # 如果已经再次扫描了,或者扫描的角度小于82，就直接返回对应的数据
                else:
                    # 将默认的卷积的次数重新设置为5
                    jiaocha_ruku_H_line_iterations = 5

                    # 由于函数需要多次调用，所以将标志位清零
                    jiaocha_ruku_H_line_detect_flag = 0

                    # 返回扫描进程已经结束的标志位
                    finish_flag = 1

                    # 打印部分数据用于调试
                    print('slope is : ', H_slope)
                    return H_points_1, H_points_2, jiaocha_ruku_H_line, H_slope, finish_flag

            else:
                print('FFFFFFFFFFFFFFind_line_but_not_H_Longitudinal')
                # 就返回一个空值
                points_1_none = (0, 0)
                points_2_none = (0, 0)
                blue_line_none = None
                # 返回扫描进程已经结束的标志位
                finish_flag = 1

                # 由于函数需要多次调用，所以将标志位清零
                jiaocha_ruku_H_line_detect_flag = 0

                return points_1_none, points_2_none, blue_line_none, None, finish_flag




        # 斜率如果为空，就报错
        else:
            print('The slope is None')
            # 就返回一个空值
            points_1_none = (0, 0)
            points_2_none = (0, 0)
            blue_line_none = None

            # 返回扫描进程已经结束的标志位
            finish_flag = 1

            # 由于函数需要多次调用，所以将标志位清零
            jiaocha_ruku_H_line_detect_flag = 0
            return points_1_none, points_2_none, blue_line_none, None, finish_flag


# 下面是斜率巡线的控制部分，就是扫描竖线，然后不停的向后倒车
def jiaocha_slope_line_detect_control(img):
    # 斜率巡线的具体参数
    global jiaocha_ruku_line, jiaocha_ruku_H_slope

    # 首先获得二值图像，用于斜率巡线处理
    bina_img = Capture_lane_image.get_lane_image(img)

    # 之后进行斜率的扫描
    jc_points_1, jc_points_2, jiaocha_ruku_line, jiaocha_ruku_H_slope, jiaocharuku_H_line_finish_flag = jiaocha_ruku_H_line_detect(
        bina_img)

    # 老规矩，先判断扫线流程是否结束
    if jiaocharuku_H_line_finish_flag == 1:
        print('jiaocha_ruku_finish_flag is 1')

        # 再判断扫线是否有有效的结果
        if jiaocha_ruku_line is not None:
            print('jiaocha_ruku_line is normal')

            # 再根据扫描的斜率发布巡线的指令
            if jiaocha_ruku_H_slope > 0:

                # 计算获得角度的差值
                slope_error = 90 - jiaocha_ruku_H_slope

                angle_jc_ruku = slope_error * 2

                print('angle_jc_ruku: ', -angle_jc_ruku - 3)
                # 左后巡线
                # 直接给出巡线角度
                msg.drive.steering_angle = -angle_jc_ruku - 3
                # 给出巡线的速度（倒车行驶）
                msg.drive.speed = 20 * dianji_fuhao
                # 发布消息
                pub.publish(msg)
                return 0

            elif jiaocha_ruku_H_slope < 0:

                # 计算获得角度的差值
                slope_error = 90 + jiaocha_ruku_H_slope

                angle_jc_ruku = slope_error * 2

                print('angle_jc_ruku: ', angle_jc_ruku)
                # 右后巡线
                # 直接给出巡线角度
                msg.drive.steering_angle = angle_jc_ruku - 4
                # 给出巡线的速度（倒车行驶）
                msg.drive.speed = 20 * dianji_fuhao
                # 发布消息
                pub.publish(msg)
                return 0

        # 如果没有扫描到线
        else:
            print('H_F_JC_ruku_line is NNNNNNNone')
            msg.drive.steering_angle = -3
            # 缓慢向后行驶
            msg.drive.speed = 0
            pub.publish(msg)
            return 0
    # 如果线段的扫描还没有结束，那就啥都不管
    else:
        print('JC_ruku_finish_flag is 0')
        # 啥都不做
        return 0


# 下面是避障斜率巡线的控制部分，就是扫描竖线，前进
def bizhang_slope_line_detect_control(img):
    # 斜率巡线的具体参数
    global jiaocha_ruku_line, jiaocha_ruku_H_slope

    # 首先获得二值图像，用于斜率巡线处理
    bina_img = Capture_lane_image.get_lane_image(img)

    # 之后进行斜率的扫描
    jc_points_1, jc_points_2, jiaocha_ruku_line, jiaocha_ruku_H_slope, jiaocharuku_H_line_finish_flag = jiaocha_ruku_H_line_detect(
        bina_img)

    # 老规矩，先判断扫线流程是否结束
    if jiaocharuku_H_line_finish_flag == 1:
        print('jiaocha_ruku_finish_flag is 1')

        # 再判断扫线是否有有效的结果
        if jiaocha_ruku_line is not None:
            print('jiaocha_ruku_line is normal')

            # 再根据扫描的斜率发布巡线的指令
            if jiaocha_ruku_H_slope > 0:

                # 计算获得角度的差值
                slope_error = 90 - jiaocha_ruku_H_slope

                angle_jc_ruku = slope_error * 2

                print('angle_jc_ruku: ', angle_jc_ruku - 3)
                # 左后巡线
                # 直接给出巡线角度
                msg.drive.steering_angle = angle_jc_ruku - 3
                # 给出巡线的速度（前进行驶）
                msg.drive.speed = -20 * dianji_fuhao
                # 发布消息
                pub.publish(msg)
                return 0

            elif jiaocha_ruku_H_slope < 0:

                # 计算获得角度的差值
                slope_error = 90 - jiaocha_ruku_H_slope

                angle_jc_ruku = slope_error * 2

                print('angle_jc_ruku: ', -angle_jc_ruku)
                # 右后巡线
                # 直接给出巡线角度
                msg.drive.steering_angle = -angle_jc_ruku - 4
                # 给出巡线的速度（前进行驶）
                msg.drive.speed = -20 * dianji_fuhao
                # 发布消息
                pub.publish(msg)
                return 0

        # 如果没有扫描到线
        else:
            print('H_F_JC_ruku_line is NNNNNNNone')
            msg.drive.steering_angle = -3
            # 缓慢向后行驶
            msg.drive.speed = 0
            pub.publish(msg)
            return 0
    # 如果线段的扫描还没有结束，那就啥都不管
    else:
        print('JC_ruku_finish_flag is 0')
        # 啥都不做
        return 0


# 中间往右边扫描的特殊扫描，用于避障
def bizhang_zhong_xian_line_detect_control(img):
    print('enter_bizhang_zhong_xian_line_detect_control')
    # 先拿到巡线用的二值图
    bina_img = Capture_lane_image.get_lane_image(img)

    # 直接用相关巡线函数,找到右边界
    left_bound_445, right_bound_445 = wig_lane_dec.boundary_detect(bina_img)

    # 算出中点和右边界的差值

    left_error = 320 - left_bound_445
    right_error = right_bound_445 - 320
    print('right_error:', right_error)
    print('left_error:', left_error)
    # 如果右边扫不到东西，返回的结果是320，那就扫左边界，也有停止条件
    if right_error == 320:

        # print('left_error:', left_error)
        # 如果左边扫描小于20，也停止
        if left_error < 20:
            return 1

        # 如果左边界在一个合理的值，那就车头往左，找虚线
        elif 20 < left_error < 200:
            # 往左边走
            msg.drive.steering_angle = -20
            msg.drive.speed = -15 * dianji_fuhao
            pub.publish(msg)
            return 0

        # 啥东西都找不到，就默认向前走
        else:
            # 往前走
            msg.drive.steering_angle = -6
            msg.drive.speed = -15 * dianji_fuhao
            pub.publish(msg)
            return 0

    # 如果找到右边界了
    else:

        # 如果右边界大于200，属于无效情况，往左边走
        if right_error > 200:
            msg.drive.steering_angle = -20
            msg.drive.speed = -15 * dianji_fuhao
            pub.publish(msg)
            return 0
        # 如果扫描的右边界正常，就正常往右走
        else:
            msg.drive.steering_angle = right_error
            msg.drive.speed = -15 * dianji_fuhao
            pub.publish(msg)

            # 右边界的停止条件
            if right_error <= 10:
                return 1
                # return 0
            return 0


# 当车次听到最后一条蓝线的位置之后，需要根据情况执行一些动作组---------------------------


# 倒车——左+右（修正，不压线）
def action_back_left_to_right():
    # 动作组直行标志位
    global action_l_t_r

    # 代表动作组结束的流程（调试的时候使用）
    if action_l_t_r == -1:
        print('action_l_t_r is finish')
        msg.drive.steering_angle = -3
        msg.drive.speed = 0
        pub.publish(msg)

        return 1

    if action_l_t_r == 0:
        # 左前
        print('left_front_1')
        msg.drive.steering_angle = -20
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(4)

        action_l_t_r = 1
        return 0

    elif action_l_t_r == 1:
        # 右后
        print('right_back_2')
        msg.drive.steering_angle = 20
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(4)

        action_l_t_r = 2
        return 0


    elif action_l_t_r == 2:
        # 直退
        print('straight_back_3')
        msg.drive.steering_angle = -3
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1.5)

        action_l_t_r = 3
        return 0

    elif action_l_t_r == 3:
        # 左后1
        print('left_back_4')
        msg.drive.steering_angle = -20
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(4)

        action_l_t_r = 4
        return 0

    elif action_l_t_r == 4:
        # 右前1
        print('right_front_5')
        msg.drive.steering_angle = 20
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1.5)

        action_l_t_r = 5
        return 0

    elif action_l_t_r == 5:
        # 左后2
        print('left_back_6')
        msg.drive.steering_angle = -20
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1.5)

        action_l_t_r = 6
        return 0

    elif action_l_t_r == 6:
        # 右前2
        print('right_front_7')
        msg.drive.steering_angle = 20
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1)

        # 标志位增加，回调进入下一个阶段
        action_l_t_r = 7
        return 0

    elif action_l_t_r == 7:
        # 停止
        print('Stop_7')
        msg.drive.steering_angle = -3
        msg.drive.speed = 0
        pub.publish(msg)

        # 重置标志位
        action_l_t_r = 0

        return 1


# 倒车——右+左(修正，不压线)
def action_back_right_to_left():
    # 动作组直行标志位
    global action_r_t_l

    # 代表动作组结束的流程（调试的时候使用）
    if action_r_t_l == -1:
        print('action_l_t_r is finish')
        msg.drive.steering_angle = -3
        msg.drive.speed = 0
        pub.publish(msg)

        return 1

    if action_r_t_l == 0:
        # 右前
        print('right_front_1')
        msg.drive.steering_angle = 20
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(4)

        action_r_t_l = 1
        return 0

    elif action_r_t_l == 1:
        # 左后
        print('left_back_2')
        msg.drive.steering_angle = -20
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(4)

        action_r_t_l = 2
        return 0


    elif action_r_t_l == 2:
        # 直退
        print('straight_back_3')
        msg.drive.steering_angle = -3
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1.5)

        action_r_t_l = 3
        return 0

    elif action_r_t_l == 3:
        # 右后1
        print('right_back_4')
        msg.drive.steering_angle = 20
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(4)

        action_r_t_l = 4
        return 0

    elif action_r_t_l == 4:
        # 左前1
        print('left_front_5')
        msg.drive.steering_angle = -20
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1.5)

        action_r_t_l = 5
        return 0

    elif action_r_t_l == 5:
        # 右后2
        print('right_back_6')
        msg.drive.steering_angle = 20
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1.5)

        # 标志位增加，回调进入下一个阶段
        action_r_t_l = 6
        return 0

    elif action_r_t_l == 6:
        # 左前2
        print('left_front_7')
        msg.drive.steering_angle = -20
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1)

        # 标志位增加，回调进入下一个阶段
        action_r_t_l = 7
        return 0

    elif action_r_t_l == 7:
        # 停止
        print('Stop_7')
        msg.drive.steering_angle = -3
        msg.drive.speed = 0
        pub.publish(msg)

        # 重置标志位
        action_r_t_l = 0

        return 1


# 执行交叉倒车入库的执行函数，需要根据第一条蓝线后退，需要扫描蓝线的函数
# 这里给了要执行（右——左：1）（左——右：0）
def jiaocha_daoche_ruku_Execute_control(img, direct_select):
    # 交叉入库的执行标志位
    global JC_daoche_ruku_Execute_flag

    print('JC_daoche_ruku_Execute_flag:', JC_daoche_ruku_Execute_flag)
    # 跳出的函数
    if JC_daoche_ruku_Execute_flag == -1:
        print('JC_Execute_is finish')
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        return 1

    # 第一步
    elif JC_daoche_ruku_Execute_flag == 0:
        # 首先进行蓝线后退的操作
        back_flag = Back_blue_line(img)

        # 如何符合结束要求，则进入下一个阶段
        if back_flag == 1:
            JC_daoche_ruku_Execute_flag = 1

        else:
            JC_daoche_ruku_Execute_flag = 0

        return 0

    # 第二步
    elif JC_daoche_ruku_Execute_flag == 1:
        # 接下来进行巡线的操作
        # 这个函数返回的标志位一定是0，暂时不接收了
        jiaocha_slope_line_detect_control(img)

        # 同时呢，也进行扫描蓝线的操作，这里就使用到了蓝线模块
        JC_ruku_blue_line_flag = stop_by_blue_line(img)

        # 如果检测到了蓝线，那么就进入下一步操作
        if JC_ruku_blue_line_flag == 1:
            JC_daoche_ruku_Execute_flag = 2

        else:
            JC_daoche_ruku_Execute_flag = 1

        return 0

    # 第三步，进行动作组，这里的动作组可以进行选择
    elif JC_daoche_ruku_Execute_flag == 2:

        # 根据外部参数进行动作选择
        if direct_select == 0:

            # 进行左到右的动作组调整操作
            action_ltr_flag = action_back_left_to_right()

            if action_ltr_flag == 1:
                JC_daoche_ruku_Execute_flag = -1

            else:
                JC_daoche_ruku_Execute_flag = 2


        elif direct_select == 1:

            # 进行右到左的动作组调整操作
            action_rtl_flag = action_back_right_to_left()

            if action_rtl_flag == 1:
                JC_daoche_ruku_Execute_flag = -1

            else:
                JC_daoche_ruku_Execute_flag = 2

        return 0


# 下面是交叉入库正车的入库部分
# 正车 左——右（修正2，不压线）
def action_front_left_to_right():
    # 动作组直行标志位
    global action_l_t_r_front

    # 代表动作组结束的流程（调试的时候使用）
    if action_l_t_r_front == -1:
        print('action_l_t_r_front is finish')
        msg.drive.steering_angle = -3
        msg.drive.speed = 0
        pub.publish(msg)

        return 1

    if action_l_t_r_front == 0:

        # 前进越过蓝线
        print('straight_front_1')
        msg.drive.steering_angle = -3
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(4.5)

        action_l_t_r_front = 1
        return 0

    elif action_l_t_r_front == 1:
        # 右前
        print('right_front_2')
        msg.drive.steering_angle = 20
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(4)

        action_l_t_r_front = 2
        return 0


    elif action_l_t_r_front == 2:
        # 左后
        print('left_back_3')
        msg.drive.steering_angle = -20
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(4)

        action_l_t_r_front = 3
        return 0

    elif action_l_t_r_front == 3:
        # 直行
        print('straight_front_4')
        msg.drive.steering_angle = -3
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(7)

        action_l_t_r_front = 4
        return 0

    elif action_l_t_r_front == 4:
        # 左前1
        print('left_front_5')
        msg.drive.steering_angle = -20
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1.5)

        action_l_t_r_front = 5
        return 0

    elif action_l_t_r_front == 5:
        # 右后1
        print('right_back_6')
        msg.drive.steering_angle = 20
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1)

        # 标志位增加，回调进入下一个阶段
        action_l_t_r_front = 6
        return 0

    elif action_l_t_r_front == 6:
        # 左前2
        print('left_front_7')
        msg.drive.steering_angle = -20
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1.5)

        action_l_t_r_front = 7
        return 0

    elif action_l_t_r_front == 7:
        # 右后2
        print('right_back_8')
        msg.drive.steering_angle = 20
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(2.5)

        # 标志位增加，回调进入下一个阶段
        action_l_t_r_front = 8
        return 0

    elif action_l_t_r_front == 8:
        # 进行一次倒退
        print('back_9')
        msg.drive.steering_angle = -3
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1)

        # 标志位增加，回调进入下一个阶段
        action_l_t_r_front = 9
        return 0

    elif action_l_t_r_front == 9:
        # 停止
        print('Stop_8')
        msg.drive.steering_angle = -3
        msg.drive.speed = 0
        pub.publish(msg)

        # 重置标志位
        action_l_t_r_front = 0

        return 1


# 正车 右——左（修正4，不压线）
def action_front_right_to_left():
    # 动作组直行标志位
    global action_r_t_l_front

    # 代表动作组结束的流程（调试的时候使用）
    if action_r_t_l_front == -1:
        print('action_r_t_l_front is finish')
        msg.drive.steering_angle = -3
        msg.drive.speed = 0
        pub.publish(msg)

        return 1

    if action_r_t_l_front == 0:
        # 前进越过蓝线
        print('straight_front_1')
        msg.drive.steering_angle = -6
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(4.5)

        action_r_t_l_front = 1
        return 0

    elif action_r_t_l_front == 1:
        # 左前
        print('left_front_2')
        msg.drive.steering_angle = -20
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(4.5)

        action_r_t_l_front = 2
        return 0


    elif action_r_t_l_front == 2:
        # 右后
        print('right_back_3')
        msg.drive.steering_angle = 20
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(4.5)

        action_r_t_l_front = 3
        return 0

    elif action_r_t_l_front == 3:
        # 直行
        print('straight_back_444')
        msg.drive.steering_angle = -3
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(7.5)

        action_r_t_l_front = 4
        return 0

    elif action_r_t_l_front == 4:
        # 右前1
        print('right_front_5')
        msg.drive.steering_angle = 20
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1.5)

        action_r_t_l_front = 5
        return 0

    elif action_r_t_l_front == 5:
        # 左后1
        print('left_back_6')
        msg.drive.steering_angle = -20
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1)

        # 标志位增加，回调进入下一个阶段
        action_r_t_l_front = 6
        return 0

    elif action_r_t_l_front == 6:
        # 右前2
        print('right_front_7')
        msg.drive.steering_angle = 20
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1.5)

        action_r_t_l_front = 7
        return 0

    elif action_r_t_l_front == 7:
        # 左后2
        print('left_back_8')
        msg.drive.steering_angle = -20
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1.5)

        # 标志位增加，回调进入下一个阶段
        action_r_t_l_front = 8
        return 0

    elif action_r_t_l_front == 8:
        # 右前3
        print('right_front_9')
        msg.drive.steering_angle = 20
        msg.drive.speed = -20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1)

        action_r_t_l_front = 9
        return 0

    elif action_r_t_l_front == 9:
        # 左后3
        print('left_back_8')
        msg.drive.steering_angle = -20
        msg.drive.speed = 20 * dianji_fuhao
        pub.publish(msg)
        rospy.sleep(1.5)

        # 标志位增加，回调进入下一个阶段
        action_r_t_l_front = 10
        return 0

    elif action_r_t_l_front == 10:
        # 平移
        pingxing_fix_left_small(left_back_time=3,
                                right_back_time=3,
                                straight_fix_time=6)

        # 标志位增加，回调进入下一个阶段
        action_r_t_l_front = 11
        return 0

    elif action_r_t_l_front == 11:
        # 停止
        print('Stop_11')
        msg.drive.steering_angle = -3
        msg.drive.speed = 0
        pub.publish(msg)

        # 重置标志位
        action_r_t_l_front = 0

        return 1


# -----------------------------------------------------------进入元素判断函数
# 避障判断(内)
def bizhang_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global bizhang_callback_data  # 导入避障请求
    global road_type  # 导入road_type
    global data_cmd  # 导入命令数据

    '''
    避障的思路是：   不断接收来自雷达节点的距离信息
                    当距离小于一定值时，更新road_type为BIZHANG，并且将元素表索引加一
                    同时向雷达节点更新park_cmd,为9即将下位机控制权交给雷达节点
                    此时主节点不对下位机进行控制
                    当雷达节点认为已经避障结束了，则先向主节点发送避障结束
                    主节点在bizhang_control中一旦接到这个标志
                    立马向雷达节点发送park_cmd为0，即将控制权交回给下位机
                    同时更新road_type为XUNXIAN,使其下次可以正常进行元素识别和巡线
    '''
    # 想雷达结点发送消息，开始识别避障
    data_cmd.data = 1
    cmd_pub.publish(data_cmd)

    # 检测到障碍物足够近，就进行一个写死的两步操作
    if bizhang_callback_data == 1:
        data_cmd.data = 0
        cmd_pub.publish(data_cmd)

        # # 先进行倒车操作
        # msg.drive.speed = 25 * dianji_fuhao
        # msg.drive.steering_angle = 0
        # pub.publish(msg)
        # rospy.sleep(2.5)

        # 停下来
        bizhang_callback_data = 0
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)

        # 最后进入避障控制
        road_type = Road_Type.BIZHANG


# 避障判断(外)
def bizhang_wai_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global bizhang_callback_data  # 导入避障请求
    global road_type  # 导入road_type
    global data_cmd  # 导入命令数据

    '''
    避障的思路是：   不断接收来自雷达节点的距离信息
                    当距离小于一定值时，更新road_type为BIZHANG，并且将元素表索引加一
                    同时向雷达节点更新park_cmd,为9即将下位机控制权交给雷达节点
                    此时主节点不对下位机进行控制
                    当雷达节点认为已经避障结束了，则先向主节点发送避障结束
                    主节点在bizhang_control中一旦接到这个标志
                    立马向雷达节点发送park_cmd为0，即将控制权交回给下位机
                    同时更新road_type为XUNXIAN,使其下次可以正常进行元素识别和巡线
    '''
    # 想雷达结点发送消息，开始识别避障
    data_cmd.data = 1
    cmd_pub.publish(data_cmd)

    # 检测到障碍物足够近，就进行一个写死的两步操作
    if bizhang_callback_data == 1:
        data_cmd.data = 0
        cmd_pub.publish(data_cmd)

        # # 先进行倒车操作
        # msg.drive.speed = 25 * dianji_fuhao
        # msg.drive.steering_angle = 0
        # pub.publish(msg)
        # rospy.sleep(2.5)

        # 停下来
        bizhang_callback_data = 0
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)

        # 最后进入避障控制
        road_type = Road_Type.BIZHANG_WAI


# 红绿灯判断
def honglvdengshibie_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type
    global honglvdeng_shibie_score
    RGD_img = img
    honglvdeng_shibie_flag = RGD.light_detection(RGD_img)
    print('RG_flag:', honglvdeng_shibie_flag)
    print('RG_score:', honglvdeng_shibie_score)
    if honglvdeng_shibie_flag == 1:
        honglvdeng_shibie_score += 1
        if honglvdeng_shibie_score >= 10:
            honglvdeng_shibie_score = 0
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            road_type = Road_Type.HONGLVDENG
    else:
        honglvdeng_shibie_score = 0
    pass


# 掉头判断
def diaotou_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type

    diaotou_img = img
    diaotou_shibie_flag = stop_by_blue_line(diaotou_img)
    if diaotou_shibie_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.DIAOTOU
        # rospy.sleep(2)    为了时间，这里省去


# 掉头判断
def diaotou_boundary_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type

    diaotou_img = img
    diaotou_shibie_flag = stop_by_blue_line(diaotou_img)
    if diaotou_shibie_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.DIAOTOU_BOUNDARY
        # rospy.sleep(2)    为了时间，这里省去


# 左大弯判断
def left_big_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type

    left_big_img = img
    # 进行蓝线的检测，并且给出标志位
    left_bule1_flag = stop_by_blue_line(left_big_img)
    if left_bule1_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.LEFT_BIG
        # rospy.sleep(2)    为了时间，这里省去


# （快速）左大弯判断
def left_big_fast_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type

    left_big_fast_img = img
    # 进行蓝线的检测，并且给出标志位
    left_bule1_flag = stop_by_blue_line(left_big_fast_img)
    if left_bule1_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.LEFT_BIG_FAST
        # rospy.sleep(2)    为了时间，这里省去
    pass


# （边界参数）左大弯判断
def left_big_border_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type

    left_big_border_img = img
    # 进行蓝线的检测，并且给出标志位
    left_bule1_flag = stop_by_blue_line(left_big_border_img)
    if left_bule1_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.LEFT_BIG_BORDER
        # rospy.sleep(2)    为了时间，这里省去
    pass


# 左小弯判断
def left_small_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type
    left_small_img = img
    # 进行蓝线的检测，并且给出标志位
    left_blue2_flag = stop_by_blue_line(left_small_img)
    if left_blue2_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.LEFT_SMALL
        # rospy.sleep(2)    为了时间，这里省去


# (快速)左小弯判断
def left_small_fast_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type
    left_small_fast_img = img
    # 进行蓝线的检测，并且给出标志位
    left_blue2_flag = stop_by_blue_line(left_small_fast_img)
    if left_blue2_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.LEFT_SMALL_FAST
        # rospy.sleep(2)    为了时间，这里省去
    pass


# 右大弯判断
def right_big_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type

    right_big_img = img
    # 进行蓝线的检测，并且给出标志位
    right_blue1_flag = stop_by_blue_line(right_big_img)
    if right_blue1_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.RIGHT_BIG
        # rospy.sleep(2)    为了时间，这里省去


# （快速）右大弯判断
def right_big_fast_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type

    right_big_fast_img = img
    # 进行蓝线的检测，并且给出标志位
    right_blue1_flag = stop_by_blue_line(right_big_fast_img)
    if right_blue1_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.RIGHT_BIG_FAST
        # rospy.sleep(2)    为了时间，这里省去
    pass


# （边界参数）右大弯判断
def right_big_border_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type

    right_big_border_img = img
    # 进行蓝线的检测，并且给出标志位
    right_blue1_flag = stop_by_blue_line(right_big_border_img)
    if right_blue1_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.RIGHT_BIG_BORDER
        # rospy.sleep(2)    为了时间，这里省去
    pass


# 右小弯判断
def right_small_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type
    right_small_img = img
    # 进行蓝线的检测，并且给出标志位
    right_blue1_flag = stop_by_blue_line(right_small_img)
    if right_blue1_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.RIGHT_SMALL
        # rospy.sleep(2)    为了时间，这里省去


# (快速)右小弯判断
def right_small_fast_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type
    right_small_fast_img = img
    # 进行蓝线的检测，并且给出标志位
    right_blue1_flag = stop_by_blue_line(right_small_fast_img)
    if right_blue1_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.RIGHT_SMALL_FAST
        # rospy.sleep(2)    为了时间，这里省去
    pass


# 直行判断1
def straight_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type
    global straight_shibie_score  # 导入直走积分

    straight_img = img
    # 进行蓝线的检测，并且给出标志位
    straight_bule1_flag = stop_by_blue_line(straight_img)
    if straight_bule1_flag == 1:
        print("!!!!!!!!!first_straight")
        print('sleep_1s')
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.STRAIGHT
        # rospy.sleep(2)    为了时间，这里省去


# 直行判断2(额外)
def straight_judge_extra(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type
    straight_extra_img = img
    # 进行蓝线的检测，并且给出标志位
    straight_bule_extra_flag = stop_by_blue_line(straight_extra_img)
    # 为了避免白屏的情况，这里要检测到三次才算检测成功
    if straight_bule_extra_flag == 1:
        print("!!!!!!!!!extra_straight")
        print('sleep_1s')
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.STRAIGHT_EXTRA
        # rospy.sleep(1)    为了时间，这里省去


# 普通倒车入库--从右——左 扫描牌子
def daocheruku_r2l_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type
    global daoche_ruku_shibie_score  # 导入倒车入库积分

    daocheruku_img = img
    daocheruku_flag = stop_by_blue_line(daocheruku_img)
    if daocheruku_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.DAOCHERUKU_r2l
        # rospy.sleep(2)    为了时间，这里省去


# 普通倒车入库--从左——右 扫描牌子
def daocheruku_l2r_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type
    global daoche_ruku_shibie_score  # 导入倒车入库积分

    daocheruku_img = img
    daocheruku_flag = stop_by_blue_line(daocheruku_img)
    if daocheruku_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.DAOCHERUKU_l2r
        # rospy.sleep(2)    为了时间，这里省去


# 普通正车入库--从右——左 扫描牌子
def zhengcheruku_r2l_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type

    zhengcheruku_img = img
    zhengcheruku_shibie_flag = stop_by_blue_line(zhengcheruku_img)
    if zhengcheruku_shibie_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.ZHENGCHERUKU_r2l
        # rospy.sleep(2)    为了时间，这里省去


# 普通正车入库--从左——右 扫描牌子
def zhengcheruku_l2r_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type

    zhengcheruku_img = img
    zhengcheruku_shibie_flag = stop_by_blue_line(zhengcheruku_img)
    if zhengcheruku_shibie_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        road_type = Road_Type.ZHENGCHERUKU_l2r
        # rospy.sleep(2)    为了时间，这里省去


# 交叉倒车入库（右——左）      # 这个方向是车子正对的方向
def jiaocha_ruku_back_rtl_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type

    jiaocha_ruku_back_rtl_img = img
    # 进行蓝线的检测，并且给出标志位
    jiaocha_ruku_back_rtl_flag = stop_by_blue_line(jiaocha_ruku_back_rtl_img)
    if jiaocha_ruku_back_rtl_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        # 进入交叉入库的道路模式
        road_type = Road_Type.JC_RUKU_RTL_BACK
        # rospy.sleep(2)    为了时间，这里省去


# 交叉倒车入库（左——右）
def jiaocha_ruku_back_ltr_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type

    jiaocha_ruku_back_ltr_img = img
    # 进行蓝线的检测，并且给出标志位
    jiaocha_ruku_back_ltr_flag = stop_by_blue_line(jiaocha_ruku_back_ltr_img)
    if jiaocha_ruku_back_ltr_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        # 进入交叉入库的道路模式
        road_type = Road_Type.JC_RUKU_LTR_BACK
        # rospy.sleep(2)    为了时间，这里省去


# 交叉正车入库（左——右）
def jiaocha_ruku_front_ltr_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type

    jiaocha_ruku_front_ltr_img = img
    # 进行蓝线的检测，并且给出标志位
    jiaocha_ruku_front_ltr_flag = stop_by_blue_line(jiaocha_ruku_front_ltr_img)

    if jiaocha_ruku_front_ltr_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        # 进入交叉入库的道路模式
        road_type = Road_Type.JC_RUKU_LTR_FRONT
        # rospy.sleep(2)    为了时间，这里省去


# 交叉正车入库（右——左）
def jiaocha_ruku_front_rtl_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global road_type  # 导入road_type

    jiaocha_ruku_front_rtl_img = img
    # 进行蓝线的检测，并且给出标志位
    jiaocha_ruku_front_rtl_flag = stop_by_blue_line(jiaocha_ruku_front_rtl_img)
    if jiaocha_ruku_front_rtl_flag == 1:
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        # 进入交叉入库的道路模式
        road_type = Road_Type.JC_RUKU_RTL_FRONT
        # rospy.sleep(2)    为了时间，这里省去


# 侧方停车识别
def cefangtingche_judge(img):
    global Element_Table_Index  # 导入元素表索引
    global bizhang_callback_data  # 导入避障请求
    global road_type  # 导入road_type
    global data_cmd  # 导入命令数据
    global cefang_start_count  # 侧方停车的起始积分
    global cefang_start_time  # 侧方停车的起始时间
    global cefang_judge_flag  # 侧方停车判断标志位
    # 进入到判断函数，将侧方停车的起始积分逐步增加

    print('cefang_start_count: ', cefang_start_count)
    # cefang_start_count += 1
    if cefang_judge_flag == 1:
        cefang_start_time = time.time()
        cefang_judge_flag = 2

    elif cefang_judge_flag == 2:
        cefang_now_time = time.time()
        if cefang_now_time - cefang_start_time > 7:
            cefang_judge_flag = 3
            cefang_start_time = 0
        # 在这里加上停车牌初步识别

    elif cefang_judge_flag == 3:
        # 想雷达结点发送消息，开始识别停车牌
        data_cmd.data = 7
        cmd_pub.publish(data_cmd)

        # 检测到障碍物足够近，就进行一个写死的两步操作
        if bizhang_callback_data == 7:
            print('cefang_start_count_get: ', cefang_start_count)
            data_cmd.data = 0
            cmd_pub.publish(data_cmd)
            # 识别到停车牌停下来
            bizhang_callback_data = 0
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)

            # 最后进入侧方停车
            road_type = Road_Type.CEFANGTINGCHE


# ----------------------------------------------------------------------


# 状态机核心执行函数
# 包括了巡线的基础状态机控制
def xunxian_control(img):
    global Element_Table  # 导入元素表
    global Element_Table_Index  # 导入元素表索引
    global msg  # 导入msg,用于小车底盘控制
    global start_flag  # 在红绿灯识别中用于判断是否已经启动
    global back
    global cefang_start_count
    print('enter_xunxian_control')
    ###下面写正常循迹代码
    if start_flag == 1:
        xunxian_img = img

        # 如果当前元素是雷达避障结点，则使用一个缓慢的速度前进
        if Element_Table[Element_Table_Index] == 1:
            # 避障的时候，获得一下相关的转角，速度
            msg.drive.steering_angle, back = lane_detect_normal(xunxian_img)
            # 这里我们还是加上倒车，避障也有倒车功能了
            if back == -1:
                msg.drive.steering_angle = -msg.drive.steering_angle * 0.4
                msg.drive.speed = 20 * dianji_fuhao
                pub.publish(msg)
                print('back!')
                rospy.sleep(0.5)
            else:

                msg.drive.steering_angle, back = lane_detect_normal(xunxian_img)
                msg.drive.speed = -23 * back * dianji_fuhao
                pub.publish(msg)

        elif Element_Table[Element_Table_Index] == 11:
            # 侧方的时候，获得一下相关的转角，速度
            msg.drive.steering_angle, back = lane_detect_normal(xunxian_img)
            # 这里我们还是加上倒车，侧方也有倒车功能了
            if back == -1:
                msg.drive.steering_angle = -msg.drive.steering_angle * 0.4
                msg.drive.speed = 20 * dianji_fuhao
                pub.publish(msg)
                print('back!')
                rospy.sleep(0.5)
            else:
                # 侧方的速度，无条件30
                msg.drive.steering_angle, back = lane_detect_normal(xunxian_img)
                msg.drive.speed = -30 * back * dianji_fuhao
                pub.publish(msg)

        # 否则就正常巡线
        else:

            # 无条件获得转角，和倒车标志
            msg.drive.steering_angle, back = lane_detect_normal(xunxian_img)

            # 特殊巡线模式的处理和使用
            # -------------------------------------------------------
            # 要排除掉当前状态表索引是特殊动作组的情况
            if Element_Table_Index > 0:
                # 如果上一个元素是左大转，那么接着会进入巡线的特殊模式-寻找左边线模式
                if Element_Table[Element_Table_Index - 1] == 4:
                    msg.drive.steering_angle, back = lane_detect_find_left_line(xunxian_img)
                # 如果上一个元素是右大转，那么会进入巡线模式的特殊模式-寻找右边线模式
                elif Element_Table[Element_Table_Index - 1] == 5:
                    msg.drive.steering_angle, back = lane_detect_find_right_line(xunxian_img)
                # 如果上一个元素是左小转，那么接着会进入巡线的特殊模式-寻找左边线模式
                elif Element_Table[Element_Table_Index - 1] == 8:
                    msg.drive.steering_angle, back = lane_detect_find_right_line(xunxian_img)
                # 如果上一个元素是右小转，那么接着会进入巡线的特殊模式-寻找左边线模式
                elif Element_Table[Element_Table_Index - 1] == 9:
                    msg.drive.steering_angle, back = lane_detect_find_left_line(xunxian_img)
                else:
                    pass

            else:
                pass
            # ---------------------------------------------------------

            # 如果符合倒车条件，进行倒车的操作，角度速度都进行调整
            if back == -1:

                # 如果上一个元素是直线6，那就进入一个特殊的倒车模式，只给往左倒
                if Element_Table[Element_Table_Index - 1] == 6:
                    msg.drive.steering_angle = 20
                    msg.drive.speed = 20 * dianji_fuhao
                    pub.publish(msg)
                    print('special_straight_back!')
                    rospy.sleep(0.5)

                else:

                    msg.drive.steering_angle = -msg.drive.steering_angle * 0.4
                    msg.drive.speed = 20 * dianji_fuhao
                    pub.publish(msg)
                    print('back!')
                    rospy.sleep(0.5)
            # 符合正常巡线条件，就给正常速度和角度
            elif back == 1:
                # 如果上一个元素是左大转，那么接着会进入巡线的特殊模式-寻找左边线模式，并且会降低巡线的speed
                if Element_Table[Element_Table_Index - 1] == 4:
                    msg.drive.speed = -20 * back * dianji_fuhao
                    pub.publish(msg)
                # 如果上一个元素是右大转，那么会进入巡线模式的特殊模式-寻找右边线模式
                elif Element_Table[Element_Table_Index - 1] == 5:
                    msg.drive.speed = -20 * back * dianji_fuhao
                    pub.publish(msg)
                # 如果上一个元素是左小转，那么接着会进入巡线的特殊模式-寻找左边线模式
                elif Element_Table[Element_Table_Index - 1] == 8:
                    msg.drive.speed = -20 * back * dianji_fuhao
                    pub.publish(msg)
                # 如果上一个元素是右小转，那么接着会进入巡线的特殊模式-寻找左边线模式
                elif Element_Table[Element_Table_Index - 1] == 9:
                    msg.drive.speed = -20 * back * dianji_fuhao
                    pub.publish(msg)

                # 否则进入正常的巡线模式
                else:
                    msg.drive.speed = -40 * back * dianji_fuhao
                    pub.publish(msg)
            # 如果出现意外的情况，就以很慢的速度前进
            else:
                msg.drive.speed = -10 * dianji_fuhao
                pub.publish(msg)

    # 下面写状态改变的代码
    # 根据元素表的顺序进行元素判定
    if Element_Table[Element_Table_Index] == 1:
        # 如果元素为避障
        bizhang_judge(img)
        pass
    if Element_Table[Element_Table_Index] == 110:
        # 如果元素为外道避障
        bizhang_wai_judge(img)
        pass

    if Element_Table[Element_Table_Index] == 2:
        # 如果元素为倒车入库--从左往右扫描牌子
        daocheruku_l2r_judge(img)
        pass
    if Element_Table[Element_Table_Index] == 3:
        # 如果元素为红绿灯识别
        honglvdengshibie_judge(img)
        pass

    if Element_Table[Element_Table_Index] == 4:
        # 如果元素为左转大圈标志识别
        left_big_judge(img)
        pass
    if Element_Table[Element_Table_Index] == 44:
        # 如果元素位快速左转大圈
        left_big_fast_judge(img)
        pass
    if Element_Table[Element_Table_Index] == 444:
        # 如果元素位（边界参数）左转大圈
        left_big_border_judge(img)
        pass

    if Element_Table[Element_Table_Index] == 5:
        # 如果元素为右转大圈
        right_big_judge(img)
        pass
    if Element_Table[Element_Table_Index] == 55:
        # 如果元素为快速右转大圈
        right_big_fast_judge(img)
        pass
    if Element_Table[Element_Table_Index] == 555:
        # 如果元素为快速（边界参数）右转大圈
        right_big_border_judge(img)
        pass

    if Element_Table[Element_Table_Index] == 6:
        # 如果元素为直行1
        straight_judge(img)
        pass
    # ----------------------------------
    # 额外添加的状态机模式
    if Element_Table[Element_Table_Index] == 66:
        # 如果元素为直行2(额外)
        straight_judge_extra(img)
        pass

    if Element_Table[Element_Table_Index] == 7:
        # 如果元素为调头
        diaotou_judge(img)
        pass

    if Element_Table[Element_Table_Index] == 77:
        # 如果元素为边界参数调头
        diaotou_boundary_judge(img)
        pass

    if Element_Table[Element_Table_Index] == 8:
        # 如果元素为左转小圈
        left_small_judge(img)
        pass
    if Element_Table[Element_Table_Index] == 88:
        # 如果元素为快速左转小圈
        left_small_fast_judge(img)
        pass

    if Element_Table[Element_Table_Index] == 9:
        # 如果元素为右转小圈
        right_small_judge(img)
        pass
    if Element_Table[Element_Table_Index] == 99:
        # 如果元素为快速右转小圈
        right_small_fast_judge(img)
        pass

    if Element_Table[Element_Table_Index] == 10:
        # 如果元素为正车入库--从左往右扫描牌子
        zhengcheruku_l2r_judge(img)
        pass
    if Element_Table[Element_Table_Index] == 11:
        # 如果元素为侧方停车
        cefangtingche_judge(img)
        pass
    if Element_Table[Element_Table_Index] == 12:
        # 12.交叉倒车（右——左）
        jiaocha_ruku_back_rtl_judge(img)
        pass
    if Element_Table[Element_Table_Index] == 13:
        # 13.交叉倒车（左——右）
        jiaocha_ruku_back_ltr_judge(img)
        pass
    if Element_Table[Element_Table_Index] == 14:
        # 14.交叉正车（右——左）
        jiaocha_ruku_front_rtl_judge(img)
        pass
    if Element_Table[Element_Table_Index] == 15:
        # 15.交叉正车（左——右）
        jiaocha_ruku_front_ltr_judge(img)
        pass
    if Element_Table[Element_Table_Index] == 16:
        # 16.普通倒车————从右往左扫描牌子
        daocheruku_r2l_judge(img)
        pass
    if Element_Table[Element_Table_Index] == 17:
        # 17.普通正车————从右往左扫描牌子
        zhengcheruku_r2l_judge(img)
        pass


# ---------------------------------------------------------------------
# 控制函数的定义


# 避障的控制代码(内)
def bizhang_control(img):
    global bizhang_nei_exe_flag  # 避障所用到的全局执行标志位
    global nei_start_time  # 内道避障，所使用的全局时间
    global data_cmd  # 由主节点发给雷达节点
    global road_type
    global bizhang_callback_data
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点

    print("enter_bizhang_control")

    # 先写死一段左转
    if bizhang_nei_exe_flag == 0:
        msg.drive.speed = -23 * dianji_fuhao
        msg.drive.steering_angle = -30
        pub.publish(msg)
        rospy.sleep(4.5)

        # 记录当前时间
        nei_start_time = time.time()

        bizhang_nei_exe_flag = 1

    elif bizhang_nei_exe_flag == 1:

        # 如果时间条件满足，那就跳出去
        now_time = time.time()

        if (now_time - nei_start_time) > 6:

            bizhang_nei_exe_flag = 2

        else:

            print('enter_bizhang_xunxian')

            # 进入斜率巡线控制
            bizhang_slope_line_detect_control(img)

            # 保持
            bizhang_nei_exe_flag = 1


    elif bizhang_nei_exe_flag == 2:

        # 进行中点巡线操作
        zhongxian_flag = bizhang_zhong_xian_line_detect_control(img)

        if zhongxian_flag == 1:
            print('bizhang_zhong_dian_end')
            bizhang_nei_exe_flag = 3

    elif bizhang_nei_exe_flag == 3:

        msg.drive.speed = 0 * dianji_fuhao
        msg.drive.steering_angle = -6
        pub.publish(msg)
        rospy.sleep(3.5)

        print('enter_turn_right11111111111111111111111111111111111111111111111111111111111111111111')

        # 右转
        msg.drive.speed = -23 * dianji_fuhao
        msg.drive.steering_angle = 25
        pub.publish(msg)
        rospy.sleep(3.5)
        #
        # # 直行
        # msg.drive.speed = -23 * dianji_fuhao
        # msg.drive.steering_angle = -6
        # pub.publish(msg)
        # rospy.sleep(1.5)

        bizhang_nei_exe_flag = 4

    elif bizhang_nei_exe_flag == 4:

        nei_start_time = 0

        bizhang_callback_data = 0

        # 避障结束，回到巡线模式
        road_type = Road_Type.XUNXIAN

        # 元素表索引增加
        Element_Table_Index += 1

        # 避障的指令调整为0，不进行任何操作
        data_cmd.data = 0
        cmd_pub.publish(data_cmd)


# 避障的控制代码（外）
def bizhang_wai_control(img):
    global data_cmd  # 由主节点发给雷达节点
    global road_type
    global bizhang_callback_data
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点

    print("enter_bizhang_control")
    # 这里是发送执行避障动作组的命令
    data_cmd.data = 14
    cmd_pub.publish(data_cmd)

    # 这里是接收到避障结束的命令
    if bizhang_callback_data == 14:
        bizhang_callback_data = 0

        # 避障结束，回到巡线模式
        road_type = Road_Type.XUNXIAN

        # 元素表索引增加
        Element_Table_Index += 1

        # 避障的指令调整为0，不进行任何操作
        data_cmd.data = 0
        cmd_pub.publish(data_cmd)


# 倒车入库的控制代码
def daocheruku_l2r_control(img):
    global road_type
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点
    global car_mode
    global bizhang_callback_data
    global daocheruku_flag
    print("enter_daocheruku_control")
    '''
    倒车入库思路大致如下：
    1.首先在daocheruku_judge中判断是否进入倒车入库模式，如果是，则将road_type设置为Road_Type.DAOCHEKU
    2.在control函数中，先将back_camera_callback_daocheruku_flag设置为1，表示接收后摄的信息。
    3.在back_camera_callback中，采集后摄获得的图片并进行处理，处理结果放在一些全局变量里方便control函数调用。
    4.在control函数中，根据后摄处理结果进行倒车入库的操作。

    '''

    print('Fix_by_daoche_ruku_blue_line_')
    if daocheruku_flag == 1:
        # 校准的控制函数
        fix_flag = fix_blue_line_control(img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            daocheruku_jiaozhun_stop = 1
        else:
            daocheruku_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if daocheruku_jiaozhun_stop == 1:
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去
            daocheruku_flag += 1
    if daocheruku_flag == 2:
        # 告诉雷达节点开始倒车入库
        data_cmd.data = 3
        cmd_pub.publish(data_cmd)

        if bizhang_callback_data == 3:
            bizhang_callback_data = 0

            # 倒车结束，回到停车模式
            road_type = Road_Type.XUNXIAN
            car_mode = Car_Mode.Stop
            # 元素表索引增加
            Element_Table_Index += 1

            # 雷达的指令调整为0，不进行任何操作
            data_cmd.data = 0
            cmd_pub.publish(data_cmd)
            daocheruku_flag = 1
    pass


# 红绿灯的控制代码
def honglvdeng_control(img):
    global road_type
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点
    global start_flag
    msg.drive.speed = -30 * dianji_fuhao
    # -------
    print("enter_RGD_control")
    # -------
    msg.drive.steering_angle = -6
    pub.publish(msg)
    rospy.sleep(1)
    print("ok")
    start_flag = 1
    road_type = Road_Type.XUNXIAN
    Element_Table_Index += 1

    pass


# 左大转
def left_big_control(img):
    global jiaodu_now  # 全局变量，记录当前角度，所有惯导都会使用
    global left_jiaodu_last  # 特指左转开始识别时的角度
    global left_jiaodu_now  # 特指左转实时角度
    global left_big_flag  # 左转大圈控制不同阶段的标志位
    global road_type  # 全局变量，记录当前道路类型，所有状态机都会使用
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点

    # 角度矫正的持续时间
    global fix_time
    left_img = img
    print("enter_left_big_control")

    # 左转的1阶段-校准
    if left_big_flag == 1:
        print('Fix_by_left_big_blue_line_')

        # 校准的控制函数
        fix_flag = fix_blue_line_control(left_img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            left_jiaozhun_stop = 1
        else:
            left_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if left_jiaozhun_stop == 1:
            left_big_flag += 1
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去

            # 前进3秒，速度为25
            msg.drive.speed = -25 * dianji_fuhao
            msg.drive.steering_angle = -6
            pub.publish(msg)
            rospy.sleep(3)
            # 记录下位机陀螺仪6050的角度
            left_jiaodu_last = jiaodu_now

    # 左转的二阶段-转弯，使用底层板子的6050
    elif left_big_flag == 2:
        left_jiaodu_now = jiaodu_now
        msg.drive.speed = -25 * dianji_fuhao
        msg.drive.steering_angle = -27  # 小于0代表左转
        pub.publish(msg)
        # print(abs(left_jiaodu_now - left_jiaodu_last))
        if abs(left_jiaodu_now - left_jiaodu_last) > 850:
            left_big_flag += 1
            print("!!!!!")

    # 左转的三阶段-结束动作
    elif left_big_flag == 3:
        # 道路模式回到巡线
        road_type = Road_Type.XUNXIAN
        # 状态元素所以+1
        Element_Table_Index += 1
        # 重置左转标志位
        left_big_flag = 1
        # 给个0的速度
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
    pass


# 快速左大转，无特殊巡线
def left_big_fast_control(img):
    global jiaodu_now  # 全局变量，记录当前角度，所有惯导都会使用
    global left_jiaodu_last  # 特指左转开始识别时的角度
    global left_jiaodu_now  # 特指左转实时角度
    global left_big_flag_fast  # 左转大圈控制不同阶段的标志位
    global road_type  # 全局变量，记录当前道路类型，所有状态机都会使用
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点

    # 角度矫正的持续时间
    global fix_time
    left_img = img
    print("enter_left_big_fast_control")

    # 左转的1阶段-校准
    if left_big_flag_fast == 1:
        print('Fix_by_left_big_fast_blue_line_')

        # 校准的控制函数
        fix_flag = fix_blue_line_control(left_img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            left_jiaozhun_stop = 1
        else:
            left_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if left_jiaozhun_stop == 1:
            left_big_flag_fast += 1
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去

            # 前进3秒，速度为25
            msg.drive.speed = -25 * dianji_fuhao
            msg.drive.steering_angle = -6
            pub.publish(msg)
            rospy.sleep(3)
            # 记录下位机陀螺仪6050的角度
            left_jiaodu_last = jiaodu_now

    # 左转的二阶段-转弯，使用底层板子的6050
    elif left_big_flag_fast == 2:
        left_jiaodu_now = jiaodu_now
        msg.drive.speed = -25 * dianji_fuhao
        msg.drive.steering_angle = -27  # 小于0代表左转
        pub.publish(msg)
        # print(abs(left_jiaodu_now - left_jiaodu_last))
        if abs(left_jiaodu_now - left_jiaodu_last) > 850:
            left_big_flag_fast += 1
            print("!!!!!")

    # 左转的三阶段-结束动作
    elif left_big_flag_fast == 3:
        # 道路模式回到巡线
        road_type = Road_Type.XUNXIAN
        # 状态元素所以+1
        Element_Table_Index += 1
        # 重置左转标志位
        left_big_flag_fast = 1
        # 给个0的速度
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
    pass


# （边界参数）左大转
def left_big_border_control(img):
    global jiaodu_now  # 全局变量，记录当前角度，所有惯导都会使用
    global left_jiaodu_last  # 特指左转开始识别时的角度
    global left_jiaodu_now  # 特指左转实时角度
    global left_big_flag_border  # 左转大圈控制不同阶段的标志位
    global road_type  # 全局变量，记录当前道路类型，所有状态机都会使用
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点

    # 角度矫正的持续时间
    global fix_time
    left_img = img
    print("enter_left_big_border_control")

    # 左转的1阶段-校准
    if left_big_flag_border == 1:
        print('Fix_by_left_big_fast_blue_line_')

        # 校准的控制函数
        fix_flag = fix_blue_line_control(left_img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            left_jiaozhun_stop = 1
        else:
            left_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if left_jiaozhun_stop == 1:
            left_big_flag_border += 1
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去

            # 前进2.5秒，速度为22
            msg.drive.speed = -25 * dianji_fuhao
            msg.drive.steering_angle = -6
            pub.publish(msg)
            rospy.sleep(2)
            # 记录下位机陀螺仪6050的角度
            left_jiaodu_last = jiaodu_now

    # 左转的二阶段-转弯，使用底层板子的6050
    elif left_big_flag_border == 2:
        left_jiaodu_now = jiaodu_now
        msg.drive.speed = -22 * dianji_fuhao
        msg.drive.steering_angle = -50  # 小于0代表左转，满舵
        pub.publish(msg)
        # print(abs(left_jiaodu_now - left_jiaodu_last))
        if abs(left_jiaodu_now - left_jiaodu_last) > 900:  # 这个参数是边界左大特有的
            left_big_flag_border += 1
            print("!!!!!")

    # 左转的三阶段-结束动作
    elif left_big_flag_border == 3:
        # 道路模式回到巡线
        road_type = Road_Type.XUNXIAN
        # 状态元素所以+1
        Element_Table_Index += 1
        # 重置左转标志位
        left_big_flag_border = 1
        # 给个0的速度
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
    pass


# 左小转
def left_small_control(img):
    global left_small_flag  # 左转小圈控制不同阶段的标志位
    global road_type  # 全局变量，记录当前道路类型，所有状态机都会使用
    global Element_Table_Index  # 导入元素表索引
    # 角度矫正的持续时间
    global fix_time
    left_small_img = img
    print("enter_left_small_control")

    # 右小转的1阶段-校准
    if left_small_flag == 1:
        print('Fix_by_right_small_blue_line_')

        # 校准的控制函数
        fix_flag = fix_blue_line_control(left_small_img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            left_jiaozhun_stop = 1
        else:
            left_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if left_jiaozhun_stop == 1:
            left_small_flag += 1
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去


    # 左转的二阶段-转弯
    elif left_small_flag == 2:
        left_small_flag += 1
        msg.drive.speed = -25 * dianji_fuhao
        msg.drive.steering_angle = -25  # 小于0代表左转
        pub.publish(msg)
        # 右转的时间为5s
        rospy.sleep(5)

    # 左转的三阶段，后退，再转
    elif left_small_flag == 3:
        left_small_flag += 1
        msg.drive.speed = 20 * dianji_fuhao
        msg.drive.steering_angle = -6  # 小于0代表左转
        pub.publish(msg)
        # 后退的时间为1.5s
        rospy.sleep(1.5)

        msg.drive.speed = -25 * dianji_fuhao
        msg.drive.steering_angle = -25  # 小于0代表左转
        pub.publish(msg)
        # 再次左转的时间为1.5s
        rospy.sleep(1.5)

    # 左转的第四阶段-结束动作
    elif left_small_flag == 4:
        msg.drive.speed = 0 * dianji_fuhao
        msg.drive.steering_angle = -6  # 小于0代表左转
        pub.publish(msg)
        # rospy.sleep(1)    为了时间，这里省去
        road_type = Road_Type.XUNXIAN
        Element_Table_Index += 1
        left_small_flag = 1

    pass


# 快速左小转，无特殊巡线
def left_small_fast_control(img):
    global left_small_flag_fast  # 左转小圈控制不同阶段的标志位
    global road_type  # 全局变量，记录当前道路类型，所有状态机都会使用
    global Element_Table_Index  # 导入元素表索引
    # 角度矫正的持续时间
    global fix_time
    left_small_img = img
    print("enter_left_small_fast_control")

    # 右小转的1阶段-校准
    if left_small_flag_fast == 1:
        print('Fix_by_right_small_blue_line_')

        # 校准的控制函数
        fix_flag = fix_blue_line_control(left_small_img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            left_jiaozhun_stop = 1
        else:
            left_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if left_jiaozhun_stop == 1:
            left_small_flag_fast += 1
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去


    # 左转的二阶段-转弯
    elif left_small_flag_fast == 2:
        left_small_flag_fast += 1
        msg.drive.speed = -25 * dianji_fuhao
        msg.drive.steering_angle = -25  # 小于0代表左转
        pub.publish(msg)
        # 右转的时间为5s
        rospy.sleep(5)

    # 左转的三阶段，后退，再转
    elif left_small_flag_fast == 3:
        left_small_flag_fast += 1
        msg.drive.speed = 20 * dianji_fuhao
        msg.drive.steering_angle = -6  # 小于0代表左转
        pub.publish(msg)
        # 后退的时间为1.5s
        rospy.sleep(1.5)

        msg.drive.speed = -25 * dianji_fuhao
        msg.drive.steering_angle = -25  # 小于0代表左转
        pub.publish(msg)
        # 再次左转的时间为1.5s
        rospy.sleep(1.5)

    # 左转的第四阶段-结束动作
    elif left_small_flag_fast == 4:
        msg.drive.speed = 0
        msg.drive.steering_angle = -6  # 小于0代表左转
        pub.publish(msg)
        # rospy.sleep(1)    为了时间，这里省去
        road_type = Road_Type.XUNXIAN
        Element_Table_Index += 1
        left_small_flag_fast = 1

    pass


# 右大转
def right_big_control(img):
    global jiaodu_now  # 全局变量，记录当前角度，所有惯导都会使用
    global right_jiaodu_last  # 特指右转开始识别时的角度
    global right_jiaodu_now  # 特指右转实时角度
    global right_big_flag  # 右转控制不同阶段的标志位
    global road_type  # 全局变量，记录当前道路类型，所有状态机都会使用
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点
    right_blue1_flag = 0

    right_big_img = img
    print("enter_right_big_control")

    # 右转的1阶段-校准
    if right_big_flag == 1:
        print('Fix_by_right_big_blue_line_')

        # 校准的控制函数
        fix_flag = fix_blue_line_control(right_big_img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            right_jiaozhun_stop = 1
        else:
            right_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if right_jiaozhun_stop == 1:
            right_big_flag += 1
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去

            # 前进3秒，速度为25
            msg.drive.speed = -25 * dianji_fuhao
            msg.drive.steering_angle = -6
            pub.publish(msg)
            rospy.sleep(3)
            right_jiaodu_last = jiaodu_now

    # 右转的二阶段-转弯，使用底层板子的6050
    if right_big_flag == 2:
        right_jiaodu_now = jiaodu_now
        msg.drive.speed = -25 * dianji_fuhao
        msg.drive.steering_angle = 27  # 大于0代表右转
        pub.publish(msg)
        # print(abs(left_jiaodu_now - left_jiaodu_last))
        if abs(right_jiaodu_now - right_jiaodu_last) > 800:
            right_big_flag += 1
            print("!!!!!")

    # 右转的三阶段-结束动作
    if right_big_flag == 3:
        # 道路模式回到巡线
        road_type = Road_Type.XUNXIAN
        # 状态元素所以+1
        Element_Table_Index += 1
        # 重置右转标志位
        right_big_flag = 1
        # 给个0的速度
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
    pass


# 快速右大转，无特殊巡线
def right_big_fast_control(img):
    global jiaodu_now  # 全局变量，记录当前角度，所有惯导都会使用
    global right_jiaodu_last  # 特指右转开始识别时的角度
    global right_jiaodu_now  # 特指右转实时角度
    global right_big_flag_fast  # 右转控制不同阶段的标志位
    global road_type  # 全局变量，记录当前道路类型，所有状态机都会使用
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点
    right_blue1_flag = 0

    right_big_img = img
    print("enter_right_big_fast_control")

    # 右转的1阶段-校准
    if right_big_flag_fast == 1:
        print('Fix_by_right_big_blue_line_')

        # 校准的控制函数
        fix_flag = fix_blue_line_control(right_big_img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            right_jiaozhun_stop = 1
        else:
            right_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if right_jiaozhun_stop == 1:
            right_big_flag_fast += 1
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去

            # 前进3秒，速度为25
            msg.drive.speed = -25 * dianji_fuhao
            msg.drive.steering_angle = -6
            pub.publish(msg)
            rospy.sleep(3)
            right_jiaodu_last = jiaodu_now

    # 右转的二阶段-转弯，使用底层板子的6050
    if right_big_flag_fast == 2:
        right_jiaodu_now = jiaodu_now
        msg.drive.speed = -25 * dianji_fuhao
        msg.drive.steering_angle = 27  # 大于0代表右转
        pub.publish(msg)
        # print(abs(left_jiaodu_now - left_jiaodu_last))
        if abs(right_jiaodu_now - right_jiaodu_last) > 800:
            right_big_flag_fast += 1
            print("!!!!!")

    # 右转的三阶段-结束动作
    if right_big_flag_fast == 3:
        # 道路模式回到巡线
        road_type = Road_Type.XUNXIAN
        # 状态元素所以+1
        Element_Table_Index += 1
        # 重置右转标志位
        right_big_flag_fast = 1
        # 给个0的速度
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
    pass


# 快速右大转，无特殊巡线
def right_big_border_control(img):
    global jiaodu_now  # 全局变量，记录当前角度，所有惯导都会使用
    global right_jiaodu_last  # 特指右转开始识别时的角度
    global right_jiaodu_now  # 特指右转实时角度
    global right_big_flag_border  # 右转控制不同阶段的标志位
    global road_type  # 全局变量，记录当前道路类型，所有状态机都会使用
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点

    right_big_img = img
    print("enter_right_big_fast_control")

    # 右转的1阶段-校准
    if right_big_flag_border == 1:
        print('Fix_by_right_big_blue_line_')

        # 校准的控制函数
        fix_flag = fix_blue_line_control(right_big_img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            right_jiaozhun_stop = 1
        else:
            right_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if right_jiaozhun_stop == 1:
            right_big_flag_border += 1
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去

            # 前进3秒，速度为25
            msg.drive.speed = -25 * dianji_fuhao
            msg.drive.steering_angle = -6
            pub.publish(msg)
            rospy.sleep(2.5)
            right_jiaodu_last = jiaodu_now

    # 右转的二阶段-转弯，使用底层板子的6050
    if right_big_flag_border == 2:
        right_jiaodu_now = jiaodu_now
        msg.drive.speed = -23 * dianji_fuhao
        msg.drive.steering_angle = 27  # 大于0代表右转
        pub.publish(msg)
        # print(abs(left_jiaodu_now - left_jiaodu_last))
        if abs(right_jiaodu_now - right_jiaodu_last) > 900:
            right_big_flag_border += 1
            print("!!!!!")

    # 右转的三阶段-结束动作
    if right_big_flag_border == 3:
        # 道路模式回到巡线
        road_type = Road_Type.XUNXIAN
        # 状态元素所以+1
        Element_Table_Index += 1
        # 重置右转标志位
        right_big_flag_border = 1
        # 给个0的速度
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
    pass


# 右小转
def right_small_control(img):
    global right_small_flag  # 左转小圈控制不同阶段的标志位
    global road_type  # 全局变量，记录当前道路类型，所有状态机都会使用
    global Element_Table_Index  # 导入元素表索引

    right_small_img = img
    print("enter_right_small_control")

    # 右小转的1阶段-校准
    if right_small_flag == 1:
        print('Fix_by_right_small_blue_line_')

        # 校准的控制函数
        fix_flag = fix_blue_line_control(right_small_img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            right_jiaozhun_stop = 1
        else:
            right_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if right_jiaozhun_stop == 1:
            right_small_flag += 1
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去


    # 右转的二阶段-转弯
    elif right_small_flag == 2:
        right_small_flag += 1
        msg.drive.speed = -25 * dianji_fuhao
        msg.drive.steering_angle = 20  # 大于0代表右转
        pub.publish(msg)
        # 右转的时间为5s
        rospy.sleep(5)

    # 右转的三阶段，后退，再转
    elif right_small_flag == 3:
        right_small_flag += 1
        msg.drive.speed = 20 * dianji_fuhao
        msg.drive.steering_angle = -6  # 大于0代表右转
        pub.publish(msg)
        # 后退的时间为1.5s
        rospy.sleep(1.5)

        msg.drive.speed = -25 * dianji_fuhao
        msg.drive.steering_angle = 20  # 大于0代表右转
        pub.publish(msg)
        # 再次右转的时间为1.5s
        rospy.sleep(1.5)

    # 右转的第四阶段-结束动作
    elif right_small_flag == 4:
        msg.drive.speed = 0
        msg.drive.steering_angle = -6  # 大于0代表右转
        pub.publish(msg)
        # rospy.sleep(1)    为了时间，这里省去
        road_type = Road_Type.XUNXIAN
        Element_Table_Index += 1
        right_small_flag = 1

    pass


# 快速右小转，无特殊巡线
def right_small_fast_control(img):
    global right_small_flag_fast  # 左转小圈控制不同阶段的标志位
    global road_type  # 全局变量，记录当前道路类型，所有状态机都会使用
    global Element_Table_Index  # 导入元素表索引

    right_small_img = img
    print("enter_right_small_fast_control")

    # 右小转的1阶段-校准
    if right_small_flag_fast == 1:
        print('Fix_by_right_small_blue_line_')

        # 校准的控制函数
        fix_flag = fix_blue_line_control(right_small_img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            right_jiaozhun_stop = 1
        else:
            right_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if right_jiaozhun_stop == 1:
            right_small_flag_fast += 1
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去


    # 右转的二阶段-转弯
    elif right_small_flag_fast == 2:
        right_small_flag_fast += 1
        msg.drive.speed = -25 * dianji_fuhao
        msg.drive.steering_angle = 20  # 大于0代表右转
        pub.publish(msg)
        # 右转的时间为5s
        rospy.sleep(5)

    # 右转的三阶段，后退，再转
    elif right_small_flag_fast == 3:
        right_small_flag_fast += 1
        msg.drive.speed = 20 * dianji_fuhao
        msg.drive.steering_angle = -6  # 大于0代表右转
        pub.publish(msg)
        # 后退的时间为1.5s
        rospy.sleep(1.5)

        msg.drive.speed = -25 * dianji_fuhao
        msg.drive.steering_angle = 20  # 大于0代表右转
        pub.publish(msg)
        # 再次右转的时间为1.5s
        rospy.sleep(1.5)

    # 右转的第四阶段-结束动作
    elif right_small_flag_fast == 4:
        msg.drive.speed = 0
        msg.drive.steering_angle = -6  # 大于0代表右转
        pub.publish(msg)
        # rospy.sleep(1)    为了时间，这里省去
        road_type = Road_Type.XUNXIAN
        Element_Table_Index += 1
        right_small_flag_fast = 1

    pass


# 直线前进1-finish
def straight_control(img):
    global jiaodu_now  # 全局变量，记录当前角度，所有惯导都会使用
    global straight_flag  # 右转控制不同阶段的标志位
    global road_type  # 全局变量，记录当前道路类型，所有状态机都会使用
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点

    global back

    straight_control_img = img
    print("enter_straight_control")

    # 第1步找到第一根蓝线后，校正角度
    if straight_flag == 1:
        print('straight_flag:1')
        print('Fix_by_straight_blue_line_1')

        # 校准的控制函数
        fix_flag = fix_blue_line_control(straight_control_img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            straight_jiaozhun_stop = 1
        else:
            straight_jiaozhun_stop = 0

        # 矫正结束之后,等一会(1s), 直接越过第一条蓝线(2.5s)，并进入直线控制的第二步
        if straight_jiaozhun_stop == 1:
            print("wait for 1s")
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去

            print('go straight for 2.5s')
            msg.drive.speed = -25 * dianji_fuhao
            msg.drive.steering_angle = -6
            pub.publish(msg)
            rospy.sleep(2.5)
            straight_flag += 1

    # 第2步，越过第二根蓝线回到巡线模式
    if straight_flag == 2:
        print('straight_flag:2')
        road_type = Road_Type.XUNXIAN
        # 状态元素所以+1
        Element_Table_Index += 1
        # 重置直行标志位
        straight_flag = 1
        # 给个0的速度
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        # # 等个5秒看看，这里不用等待了
        # print('wait_5s_in_end')
        # rospy.sleep(5)
    pass


# 额外的执行控制
def straight_control_extra(img):
    global straight_flag_extra  # 右转控制不同阶段的标志位
    global road_type  # 全局变量，记录当前道路类型，所有状态机都会使用
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点

    straight_control_extra_img = img
    print("enter_straight_extra_control")

    # 第1步找到第一根蓝线后，校正角度
    if straight_flag_extra == 1:
        print('straight_flag:1')
        print('Fix_by_straight_blue_line_2')

        # 校准的控制函数
        fix_extra_flag = fix_blue_line_control(straight_control_extra_img)

        # 校准结束则给一个已经停下来的标志位
        if fix_extra_flag == 1:
            straight_jiaozhun_extra_stop = 1
        else:
            straight_jiaozhun_extra_stop = 0

        # 矫正结束之后,等一会(1s), 直接越过第一条蓝线(2.5s)，并进入直线控制的第二步
        if straight_jiaozhun_extra_stop == 1:
            print("wait for extra 1s")
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去

            print('go straight_extra for 2.5s')
            straight_flag_extra += 1
            msg.drive.speed = -25 * dianji_fuhao
            msg.drive.steering_angle = -6
            pub.publish(msg)
            rospy.sleep(2.5)

    # 第2步，越过第二根蓝线回到巡线模式
    if straight_flag_extra == 2:
        print('straight_flag:2')
        # 道路模式回到巡线
        road_type = Road_Type.XUNXIAN
        # 状态元素所以+1
        Element_Table_Index += 1
        # 重置额外直行标志位
        straight_flag_extra = 1
        # 给个0的速度
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        # 等个5秒看看
        # 不等了
        # print('wait_5s_in_end')
        # rospy.sleep(5)
        pass


# 掉头的控制函数
def diaotou_control(img):
    global road_type
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点
    global diaotou_flag
    global tiaozheng_cishu
    print("enter_diaotou_control")

    print('Fix_by_daoche_ruku_blue_line_')
    if diaotou_flag == 1:
        # 校准的控制函数
        fix_flag = fix_blue_line_control(img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            diaotou_jiaozhun_stop = 1
        else:
            diaotou_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if diaotou_jiaozhun_stop == 1:
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去
            diaotou_flag += 1

    if diaotou_flag == 2:
        # 在这一阶段里执行几个动作组用于掉头
        # 1.往前走一段
        msg.drive.speed = -25 * dianji_fuhao
        msg.drive.steering_angle = 0
        pub.publish(msg)
        rospy.sleep(2.5)
        # 左前走一回
        msg.drive.speed = -20 * dianji_fuhao
        msg.drive.steering_angle = -15
        pub.publish(msg)
        rospy.sleep(8)
        while tiaozheng_cishu:
            # 2.左前
            msg.drive.speed = -20 * dianji_fuhao
            msg.drive.steering_angle = -50
            pub.publish(msg)
            rospy.sleep(1.5)
            # 3.右后
            msg.drive.speed = 20 * dianji_fuhao
            msg.drive.steering_angle = 50
            pub.publish(msg)
            rospy.sleep(1.5)
            tiaozheng_cishu -= 1
        # 4.左转前进一段距离
        msg.drive.speed = -20 * dianji_fuhao
        msg.drive.steering_angle = -50
        pub.publish(msg)
        rospy.sleep(7)
        # 进下一个阶段
        msg.drive.speed = 0
        msg.drive.steering_angle = 0
        pub.publish(msg)
        # rospy.sleep(1)    为了时间，这里省去
        diaotou_flag += 1

    if diaotou_flag == 3:
        # 道路模式回到巡线
        road_type = Road_Type.XUNXIAN
        # 状态元素所以+1
        Element_Table_Index += 1
        # 重置额外直行标志位
        diaotou_flag = 1
        # 重置调整次数
        tiaozheng_cishu = 7
        pass


# 边界掉头的控制函数
def diaotou_boundary_control(img):
    global road_type
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点
    global diaotou_boundary_flag
    global tiaozheng_cishu
    print("enter_diaotou_boundary_control")

    print('Fix_by_diao_tou_blue_line_')
    if diaotou_boundary_flag == 1:
        # 校准的控制函数
        fix_flag = fix_blue_line_control(img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            diaotou_jiaozhun_stop = 1
        else:
            diaotou_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if diaotou_jiaozhun_stop == 1:
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去
            diaotou_boundary_flag += 1

        # 接下来进行边界掉头相关的动作组

        # 前进
        if diaotou_boundary_flag == 2:
            msg.drive.speed = -25 * dianji_fuhao
            msg.drive.steering_angle = -6
            pub.publish(msg)
            rospy.sleep(2.5)

            diaotou_boundary_flag = 3

        # 左前1
        if diaotou_boundary_flag == 3:
            msg.drive.speed = -25 * dianji_fuhao
            msg.drive.steering_angle = -30
            pub.publish(msg)
            rospy.sleep(7)

            diaotou_boundary_flag = 4

        # 右后1
        if diaotou_boundary_flag == 4:
            msg.drive.speed = 25 * dianji_fuhao
            msg.drive.steering_angle = 30
            pub.publish(msg)
            rospy.sleep(4)

            diaotou_boundary_flag = 5

        # 左前2
        if diaotou_boundary_flag == 5:
            msg.drive.speed = -25 * dianji_fuhao
            msg.drive.steering_angle = -30
            pub.publish(msg)
            rospy.sleep(3)

            diaotou_boundary_flag = 6

        # 右后2
        if diaotou_boundary_flag == 6:
            msg.drive.speed = 25 * dianji_fuhao
            msg.drive.steering_angle = 30
            pub.publish(msg)
            rospy.sleep(2)

            diaotou_boundary_flag = 7

        # 左前3
        if diaotou_boundary_flag == 7:
            msg.drive.speed = -25 * dianji_fuhao
            msg.drive.steering_angle = -30
            pub.publish(msg)
            rospy.sleep(4)

            diaotou_boundary_flag = 8

        # 停止
        if diaotou_boundary_flag == 8:
            msg.drive.speed = 0
            msg.drive.steering_angle = -6
            pub.publish(msg)

            # 标志位重置
            diaotou_boundary_flag = 1

            # 道路模式回到巡线
            road_type = Road_Type.XUNXIAN
            # 状态元素所以+1
            Element_Table_Index += 1

    # 正车入库，从左往右扫牌子


def zhengcheruku_l2r_control(img):
    global road_type
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点
    global car_mode
    global bizhang_callback_data
    global zhengcheruku_flag
    print("enter_zhengcheruku_control")

    print('Fix_by_daoche_ruku_blue_line_')
    if zhengcheruku_flag == 1:
        # 校准的控制函数
        fix_flag = fix_blue_line_control(img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            zhengcheruku_jiaozhun_stop = 1
        else:
            zhengcheruku_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if zhengcheruku_jiaozhun_stop == 1:
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去
            zhengcheruku_flag += 1
    if zhengcheruku_flag == 2:
        # 告诉雷达节点开始正车入库
        data_cmd.data = 5
        cmd_pub.publish(data_cmd)

        if bizhang_callback_data == 5:
            bizhang_callback_data = 0

            # 正车入库结束，回到停车模式
            road_type = Road_Type.XUNXIAN
            car_mode = Car_Mode.Stop
            # 元素表索引增加
            Element_Table_Index += 1

            # 雷达的指令调整为0，不进行任何操作
            data_cmd.data = 0
            cmd_pub.publish(data_cmd)
            zhengcheruku_flag = 1
    pass


# 侧方停车控制函数
def cefangtingche_control(img):
    global data_cmd  # 由主节点发给雷达节点
    global road_type
    global car_mode
    global bizhang_callback_data
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点
    global cefangtingche_flag
    print("enter_cefangtingche_control")
    # 这里是发送执行避障动作组的命令
    if cefangtingche_flag == 1:
        data_cmd.data = 8
        cmd_pub.publish(data_cmd)
        # 这里是接收到侧方第一阶段结束的命令
        if bizhang_callback_data == 8:
            # 雷达的指令调整为0，不进行任何操作
            data_cmd.data = 0
            cmd_pub.publish(data_cmd)
            bizhang_callback_data = 0
            cefangtingche_flag = 2


    elif cefangtingche_flag == 2:
        data_cmd.data = 9  # 告诉雷达节点停止
        cmd_pub.publish(data_cmd)

        # 将雷达发给主节点的标志命令清零
        if bizhang_callback_data == 9:
            cefangtingche_flag = 3

    elif cefangtingche_flag == 3:
        # 开始执行校准的函数

        cefang_tinche_zhixing_flag = cefang_fix_Execute_control(img)

        # 结束执行校准的函数

        # 不断检测是否结束校准，如果结束
        if cefang_tinche_zhixing_flag == 1:
            bizhang_callback_data = 0
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            cefangtingche_flag = 4

    elif cefangtingche_flag == 4:
        # 侧方结束，回到巡线模式
        road_type = Road_Type.XUNXIAN
        car_mode = Car_Mode.Stop
        # 元素表索引增加
        Element_Table_Index += 1

        # 雷达的指令调整为0，不进行任何操作
        data_cmd.data = 0
        cmd_pub.publish(data_cmd)
        cefangtingche_flag = 1

    pass


# 额外增加的交叉入库控制操作
def jc_ruku_rtl_back_control(img):
    global road_type
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点
    global car_mode
    global bizhang_callback_data
    global jc_ruku_rtl_back_flag

    # 蓝线检测的积分数据
    global blue_line_score

    print("enter_jiaocha_ruku_rtl_back_control")

    print('Fix_by_jiaocha_ruku_blue_line_')
    if jc_ruku_rtl_back_flag == 0:
        # 校准的控制函数
        fix_flag = fix_blue_line_control(img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            jiaocharuku_back_rtl_jiaozhun_stop = 1
        else:
            jiaocharuku_back_rtl_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if jiaocharuku_back_rtl_jiaozhun_stop == 1:
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去

            # 由于下面的函数用到了蓝线积分数据，这里又没有大状态的切换，因此这里要手动将蓝线函数检测积分清零
            blue_line_score = 0

            jc_ruku_rtl_back_flag = 1

    # 进行一系列的位置调整操作
    elif jc_ruku_rtl_back_flag == 1:

        # 进行位置的修正
        location_fix_flag = jiaocha_daoche_ruku_Execute_control(img, 1)

        # 如果修正结束，就停止
        if location_fix_flag == 1:
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # 进入下一个执行阶段
            jc_ruku_rtl_back_flag = 2

        else:
            jc_ruku_rtl_back_flag = 1


    # 矫正结束，将控制权交给雷达结点
    elif jc_ruku_rtl_back_flag == 2:
        # 告诉雷达节点开始倒车入库
        data_cmd.data = 10
        cmd_pub.publish(data_cmd)

        if bizhang_callback_data == 10:
            bizhang_callback_data = 0

            # 倒车结束，回到停车模式
            road_type = Road_Type.XUNXIAN
            car_mode = Car_Mode.Stop

            # 雷达的指令调整为0，不进行任何操作
            data_cmd.data = 0
            cmd_pub.publish(data_cmd)
            jc_ruku_rtl_back_flag = 0

    pass


def jc_ruku_ltr_back_control(img):
    global road_type
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点
    global car_mode
    global bizhang_callback_data
    global jc_ruku_ltr_back_flag

    # 蓝线检测的积分数据
    global blue_line_score

    print("enter_jiaocha_ruku_rtl_back_control")

    print('Fix_by_jiaocha_ruku_blue_line_')
    if jc_ruku_ltr_back_flag == 0:
        # 校准的控制函数
        fix_flag = fix_blue_line_control(img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            jiaocharuku_back_ltr_jiaozhun_stop = 1
        else:
            jiaocharuku_back_ltr_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if jiaocharuku_back_ltr_jiaozhun_stop == 1:
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去

            # 由于下面的函数用到了蓝线积分数据，这里又没有大状态的切换，因此这里要手动将蓝线函数检测积分清零
            blue_line_score = 0

            jc_ruku_ltr_back_flag += 1

    # 进行一系列的位置调整操作
    elif jc_ruku_ltr_back_flag == 1:

        # 进行位置的修正
        location_fix_flag = jiaocha_daoche_ruku_Execute_control(img, 0)

        # 如果修正结束，就停止
        if location_fix_flag == 1:
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # 进入下一个执行阶段
            jc_ruku_ltr_back_flag = 2

        else:
            jc_ruku_ltr_back_flag = 1


    # 矫正结束，将控制权交给雷达结点
    elif jc_ruku_ltr_back_flag == 2:
        # 告诉雷达节点开始倒车入库
        data_cmd.data = 11
        cmd_pub.publish(data_cmd)

        if bizhang_callback_data == 11:
            bizhang_callback_data = 0

            # 倒车结束，回到停车模式
            road_type = Road_Type.XUNXIAN
            car_mode = Car_Mode.Stop

            # 雷达的指令调整为0，不进行任何操作
            data_cmd.data = 0
            cmd_pub.publish(data_cmd)
            jc_ruku_ltr_back_flag = 0

    pass


def jc_ruku_rtl_front_control(img):
    global road_type
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点
    global car_mode
    global bizhang_callback_data
    global jc_ruku_rtl_front_flag

    print("enter_jc_ruku_ltr_front_control")

    print('Fix_by_jiaocha_ruku_blue_line_')
    if jc_ruku_rtl_front_flag == 0:
        # 校准的控制函数
        fix_flag = fix_blue_line_control(img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            jiaocharuku_front_rtl_jiaozhun_stop = 1
        else:
            jiaocharuku_front_rtl_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if jiaocharuku_front_rtl_jiaozhun_stop == 1:
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去

            jc_ruku_rtl_front_flag = 1

    elif jc_ruku_rtl_front_flag == 1:
        # 执行动作组
        action_finish_flag = action_front_right_to_left()

        if action_finish_flag == 1:
            # 进入下一个阶段
            jc_ruku_rtl_front_flag = 2

        else:
            jc_ruku_rtl_front_flag = 1


    elif jc_ruku_rtl_front_flag == 2:
        # 告诉雷达节点开始正车入库
        data_cmd.data = 13
        cmd_pub.publish(data_cmd)

        if bizhang_callback_data == 13:
            bizhang_callback_data = 0

            # 正车入库结束，回到停车模式
            road_type = Road_Type.XUNXIAN
            car_mode = Car_Mode.Stop

            # 雷达的指令调整为0，不进行任何操作
            data_cmd.data = 0
            cmd_pub.publish(data_cmd)


def jc_ruku_ltr_front_control(img):
    global road_type
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点
    global car_mode
    global bizhang_callback_data
    global jc_ruku_ltr_front_flag

    print("enter_jc_ruku_ltr_front_control")

    print('Fix_by_jiaocha_ruku_blue_line_')
    if jc_ruku_ltr_front_flag == 0:
        # 校准的控制函数
        fix_flag = fix_blue_line_control(img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            jiaocharuku_front_ltr_jiaozhun_stop = 1
        else:
            jiaocharuku_front_ltr_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if jiaocharuku_front_ltr_jiaozhun_stop == 1:
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去

            jc_ruku_ltr_front_flag = 1

    elif jc_ruku_ltr_front_flag == 1:
        # 执行动作组
        action_finish_flag = action_front_left_to_right()

        if action_finish_flag == 1:
            # 进入下一个阶段
            jc_ruku_ltr_front_flag = 2

        else:
            jc_ruku_ltr_front_flag = 1

    elif jc_ruku_ltr_front_flag == 2:
        # 告诉雷达节点开始正车入库
        data_cmd.data = 12
        cmd_pub.publish(data_cmd)

        if bizhang_callback_data == 12:
            bizhang_callback_data = 0

            # 正车入库结束，回到停车模式
            road_type = Road_Type.XUNXIAN
            car_mode = Car_Mode.Stop

            # 雷达的指令调整为0，不进行任何操作
            data_cmd.data = 0
            cmd_pub.publish(data_cmd)

    pass


def daocheruku_r2l_control(img):
    global road_type
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点
    global car_mode
    global bizhang_callback_data
    global daocheruku_flag
    print("enter_daocheruku_control")
    '''
    倒车入库思路大致如下：
    1.首先在daocheruku_judge中判断是否进入倒车入库模式，如果是，则将road_type设置为Road_Type.DAOCHEKU
    2.在control函数中，先将back_camera_callback_daocheruku_flag设置为1，表示接收后摄的信息。
    3.在back_camera_callback中，采集后摄获得的图片并进行处理，处理结果放在一些全局变量里方便control函数调用。
    4.在control函数中，根据后摄处理结果进行倒车入库的操作。

    '''

    print('Fix_by_daoche_ruku_blue_line_')
    if daocheruku_flag == 1:
        # 校准的控制函数
        fix_flag = fix_blue_line_control(img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            daocheruku_jiaozhun_stop = 1
        else:
            daocheruku_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if daocheruku_jiaozhun_stop == 1:
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去
            daocheruku_flag += 1
    if daocheruku_flag == 2:
        # 告诉雷达节点开始倒车入库
        data_cmd.data = 4
        cmd_pub.publish(data_cmd)

        if bizhang_callback_data == 4:
            bizhang_callback_data = 0

            # 倒车结束，回到停车模式
            road_type = Road_Type.XUNXIAN
            car_mode = Car_Mode.Stop
            # 元素表索引增加
            Element_Table_Index += 1

            # 雷达的指令调整为0，不进行任何操作
            data_cmd.data = 0
            cmd_pub.publish(data_cmd)
            daocheruku_flag = 1
    pass


def zhengcheruku_r2l_control(img):
    global road_type
    global Element_Table_Index  # 导入元素表索引
    global msg  # 由主节点发给底盘节点
    global car_mode
    global bizhang_callback_data
    global zhengcheruku_flag
    print("enter_zhengcheruku--r2l_control")

    print('Fix_by_daoche_ruku_blue_line_')
    if zhengcheruku_flag == 1:
        # 校准的控制函数
        fix_flag = fix_blue_line_control(img)

        # 校准结束则给一个已经停下来的标志位
        if fix_flag == 1:
            zhengcheruku_jiaozhun_stop = 1
        else:
            zhengcheruku_jiaozhun_stop = 0

        # 矫正结束之后等一会(1s)
        if zhengcheruku_jiaozhun_stop == 1:
            msg.drive.speed = 0
            msg.drive.steering_angle = 0
            pub.publish(msg)
            # rospy.sleep(1)    为了时间，这里省去
            zhengcheruku_flag += 1
    if zhengcheruku_flag == 2:
        # 告诉雷达节点开始正车入库
        data_cmd.data = 6
        cmd_pub.publish(data_cmd)

        if bizhang_callback_data == 6:
            bizhang_callback_data = 0

            # 正车入库结束，回到停车模式
            road_type = Road_Type.XUNXIAN
            car_mode = Car_Mode.Stop
            # 元素表索引增加
            Element_Table_Index += 1

            # 雷达的指令调整为0，不进行任何操作
            data_cmd.data = 0
            cmd_pub.publish(data_cmd)
            zhengcheruku_flag = 1
    pass


# 小车正常模式控制执行函数
def xunji(img):
    # 状态机切换
    global road_type
    # 状态表默认索引
    global Element_Table_Index
    global Element_Table_last_Index  # (过去)

    # 蓝线检测积分
    global blue_line_score

    # 侧方停车开始的标志积分
    global cefang_start_count

    # --------------------------------------------------
    # 状态机进行状态切换的时候，关闭不需要的窗口
    if Element_Table_Index - Element_Table_last_Index != 0:
        print('destroy all windows as the Element_Table_last_Index is improved')
        cv2.destroyAllWindows()
        # 同时清空蓝线检测的积分
        blue_line_score = 0

        # 将角度设置为0
        msg.drive.steering_angle = -3

        # 将侧方开始的积分清零
        cefang_start_count = 0

    else:
        pass
    # 记录状态表的索引
    Element_Table_last_Index = Element_Table_Index
    # --------------------------------------------------

    if road_type == Road_Type.XUNXIAN:
        xunxian_control(img)
        pass
    elif road_type == Road_Type.BIZHANG:
        bizhang_control(img)
        pass
    elif road_type == Road_Type.BIZHANG_WAI:
        bizhang_wai_control(img)
        pass

    elif road_type == Road_Type.DAOCHERUKU_l2r:
        daocheruku_l2r_control(img)
        pass
    elif road_type == Road_Type.HONGLVDENG:
        honglvdeng_control(img)
        pass
    elif road_type == Road_Type.LEFT_BIG:
        left_big_control(img)
        pass
    elif road_type == Road_Type.LEFT_BIG_FAST:
        left_big_fast_control(img)
        pass
    elif road_type == Road_Type.LEFT_BIG_BORDER:
        left_big_border_control(img)
        pass

    elif road_type == Road_Type.RIGHT_BIG:
        right_big_control(img)
        pass
    elif road_type == Road_Type.RIGHT_BIG_FAST:
        right_big_fast_control(img)
        pass
    elif road_type == Road_Type.RIGHT_BIG_BORDER:
        right_big_border_control(img)
        pass

    elif road_type == Road_Type.STRAIGHT:
        straight_control(img)
        pass


    # 额外增加的一个直行控制
    elif road_type == Road_Type.STRAIGHT_EXTRA:
        straight_control_extra(img)
        pass


    elif road_type == Road_Type.DIAOTOU:
        diaotou_control(img)
        pass
    elif road_type == Road_Type.DIAOTOU_BOUNDARY:
        diaotou_boundary_control(img)
        pass

    elif road_type == Road_Type.LEFT_SMALL:
        left_small_control(img)
        pass
    elif road_type == Road_Type.LEFT_SMALL_FAST:
        left_small_fast_control(img)
        pass
    elif road_type == Road_Type.RIGHT_SMALL:
        right_small_control(img)
        pass
    elif road_type == Road_Type.RIGHT_SMALL_FAST:
        right_small_fast_control(img)
        pass

    elif road_type == Road_Type.ZHENGCHERUKU_l2r:
        zhengcheruku_l2r_control(img)
        pass
    elif road_type == Road_Type.CEFANGTINGCHE:
        cefangtingche_control(img)
        pass

    # ----------------------------------------额外增加的交叉入库
    elif road_type == Road_Type.JC_RUKU_RTL_BACK:
        jc_ruku_rtl_back_control(img)
        pass
    elif road_type == Road_Type.JC_RUKU_LTR_BACK:
        jc_ruku_ltr_back_control(img)
        pass
    elif road_type == Road_Type.JC_RUKU_RTL_FRONT:
        jc_ruku_rtl_front_control(img)
        pass
    elif road_type == Road_Type.JC_RUKU_LTR_FRONT:
        jc_ruku_ltr_front_control(img)
        pass
    elif road_type == Road_Type.DAOCHERUKU_r2l:
        daocheruku_r2l_control(img)
        pass
    elif road_type == Road_Type.ZHENGCHERUKU_r2l:
        zhengcheruku_r2l_control(img)
        pass


# 小车测试模式执行函数
def test(img):
    straight_judge(img)


# 小车停止模式执行函数
def stop():
    global speed_now
    msg.drive.speed = 0
    msg.drive.steering_angle = 0
    pub.publish(msg)
    print('speed_now', speed_now)


# 小车模式选择函数
def state_change(mode, img):
    pass
    if mode == Car_Mode.Normal:  # 0为比赛模式，正式模式
        xunji(img)
        pass
    elif mode == Car_Mode.Test:  # 1为test模式，调试模式，可以任意修改
        test(img)
        pass
    elif mode == Car_Mode.Stop:  # 2为停止模式，车辆直接停止，不接受任何控制
        stop()
        pass


# 前置摄像头回调
def front_camera_callback(data):
    # 获取小车的状态
    global car_mode
    # 将ros图像的格式转换为cv的格式
    img = CvBridge().imgmsg_to_cv2(data, "bgr8")
    # 根据
    state_change(car_mode, img)


# 避障回调
def bizhang_callback(data):
    global bizhang_callback_data
    bizhang_callback_data = data.data


# 从底层得到数据进行回调
def get_info_from_base(data):
    global jiaodu_di
    global jiaodu_gao
    global jiaodu_now
    global speed_now
    speed_now = data.linear.x * 10
    jiaodu_di = data.linear.z
    jiaodu_gao = data.linear.y
    jiaodu_gao = int(jiaodu_gao) << 8
    jiaodu_now = jiaodu_gao + int(jiaodu_di)
    if jiaodu_now > 32768:
        jiaodu_now = jiaodu_now - 65536
    else:
        jiaodu_now = jiaodu_now
    # print("get angle",jiaodu_now)


# 主节点的函数
def detector():
    global pub, cmd_pub

    # 小车参数的初始化
    Car_paragram_init()
    # 初始化主结点
    rospy.init_node('camera_cmd', anonymous=False)
    # 订阅前置摄像头
    rospy.Subscriber("/usb_cam_1/image", Image, front_camera_callback, queue_size=1, buff_size=2 ** 24)
    # 订阅雷达结点
    rospy.Subscriber("/bizhang_callback", Int32, bizhang_callback, queue_size=1)
    # 订阅下位机结点
    rospy.Subscriber("/get_speed", Twist, get_info_from_base, queue_size=1)

    # 发布与主节点速度和转向有关的指令
    pub = rospy.Publisher('/ackermann_cmd', AckermannDriveStamped, queue_size=1)
    # 发布与避障和入库有关的指令
    cmd_pub = rospy.Publisher("/park_cmd", Int32, queue_size=1)  # 用于向雷达节点发送指令
    # 从而进行避障和入库

    # 订阅循环
    rospy.spin()


# 主入口
if __name__ == "__main__":
    detector()
